tao-fake-mirror-server.c 8.89 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
// tao-fake-mirror-server.c -
//
// Implementation of a fake remote deformable mirror server.
//
//-----------------------------------------------------------------------------
//
// This file if part of TAO real-time software licensed under the MIT license
// (https://git-cral.univ-lyon1.fr/tao/tao-rt).
//
// Copyright (C) 2021-2022, Éric Thiébaut.

Éric Thiébaut's avatar
Éric Thiébaut committed
12
#include "tao-basics.h"
13
#include "tao-layouts.h"
Éric Thiébaut's avatar
Éric Thiébaut committed
14
#include "tao-remote-mirrors-private.h"
15
#include "tao-generic.h"
Éric Thiébaut's avatar
Éric Thiébaut committed
16
#include "tao-errors.h"
17

Éric Thiébaut's avatar
Éric Thiébaut committed
18
19
20
#include <math.h>
#include <string.h>

21
// Private data needed for the cleanup callback.
Éric Thiébaut's avatar
Éric Thiébaut committed
22
23
24
25
static tao_remote_mirror* dm = NULL;
static long* inds = NULL;
static uint8_t* msk = NULL;

26
27
28
29
30
31
32
// Send the requested command.
static tao_status on_send(
    tao_remote_mirror* dm,
    void* ctx)
{
    // Filter the requested commands to compute the actual commands.  Both are
    // relative to the reference commands.
33
34
35
    double* act_cmds = tao_remote_mirror_get_actual_commands(dm);
    const double* req_cmds = tao_remote_mirror_get_requested_commands(dm);
    const double* refs =  tao_remote_mirror_get_reference(dm);
Éric Thiébaut's avatar
Éric Thiébaut committed
36
    long nacts = tao_remote_mirror_get_nacts(dm);
37
38
39
40
41
42
43
    for (long i = 0; i < nacts; ++i) {
        double cmd = tao_clamp(req_cmds[i] + refs[i], -1.0, 1.0) - refs[i];
        act_cmds[i] = isfinite(cmd) ? cmd : 0.0;
    }
    return TAO_OK;
}

44
// Reset the deformable mirror.  This amounts to sending zero commands.
45
46
47
48
static tao_status on_reset(
    tao_remote_mirror* dm,
    void* ctx)
{
49
50
    double* req_cmds = tao_remote_mirror_get_requested_commands(dm);
    long nacts = tao_remote_mirror_get_nacts(dm);
51
52
53
54
55
56
    for (long i = 0; i < nacts; ++i) {
        req_cmds[i] = 0.0;
    }
    return on_send(dm, ctx);
}

57
// Release resources.  This function is automatically called on normal exit.
Éric Thiébaut's avatar
Éric Thiébaut committed
58
59
60
static void cleanup(void)
{
    if (dm != NULL) {
61
        if (dm->base.state != TAO_STATE_UNREACHABLE) {
62
            // FIXME: should lock
63
            dm->base.state = TAO_STATE_UNREACHABLE;
64
65
66
67
        }
        if (dm->base.command != TAO_COMMAND_NONE) {
            // FIXME: should lock
            dm->base.command = TAO_COMMAND_NONE;
Éric Thiébaut's avatar
Éric Thiébaut committed
68
        }
69
        tao_remote_mirror_detach(dm);
Éric Thiébaut's avatar
Éric Thiébaut committed
70
71
72
        dm = NULL;
    }
    if (msk != NULL) {
73
        tao_free(msk);
Éric Thiébaut's avatar
Éric Thiébaut committed
74
75
76
        msk = NULL;
    }
    if (inds != NULL) {
77
        tao_free(inds);
Éric Thiébaut's avatar
Éric Thiébaut committed
78
79
80
81
82
83
84
85
86
        inds = NULL;
    }
}

int main(
    int argc,
    char* argv[])
{
    // Determine program name.
87
    const char* progname = tao_basename(argv[0]);
Éric Thiébaut's avatar
Éric Thiébaut committed
88
89
90
91
92
93
94
95
96

    // Install function to free all allocated resources.
    if (atexit(cleanup) != 0) {
        fprintf(stderr, "%s: failed to install cleanup handler\n",
                progname);
        return EXIT_FAILURE;
    }

    // Parse arguments.
97
98
    char const* ident = NULL;
    bool debug = false;
99
    long nbufs = 10000;
100
    long nacts = 97;
101
102
103
    unsigned int orient = 0;
    unsigned int perms = 0077;
    const char* usage = "Usage: %s [OPTIONS ...] [--] NAME\n";
104
105
    bool opt = true;
    for (int iarg = 1; iarg < argc; ++iarg) {
106
        char dummy;
107
108
109
110
111
112
113
        if (opt) {
            // Argument may be an option.
            if (argv[iarg][0] != '-') {
                opt = false;
            } else if (argv[iarg][1] == '-' && argv[iarg][2] == '\0') {
                opt = false;
                continue;
Éric Thiébaut's avatar
Éric Thiébaut committed
114
115
            }
        }
Éric Thiébaut's avatar
Éric Thiébaut committed
116
117
118
119
        if (!opt) {
            // Positional argument.
            if (ident == NULL) {
                ident = argv[iarg];
120
                continue;
Éric Thiébaut's avatar
Éric Thiébaut committed
121
            }
Éric Thiébaut's avatar
Éric Thiébaut committed
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
            fprintf(stderr, "%s: too many arguments\n", progname);
        bad_usage:
            fprintf(stderr, usage, progname);
            return EXIT_FAILURE;
        }
        // Argument is an option.
        if (strcmp(argv[iarg], "-h") == 0
            || strcmp(argv[iarg], "-help") == 0
            || strcmp(argv[iarg], "--help") == 0) {
            printf(usage, progname);
            printf("\n");
            printf("Arguments:\n");
            printf("  NAME               Name of server.\n");
            printf("\n");
            printf("Options:\n");
            printf("  -orient BITS       Orientation of layout [%u].\n",
                   orient);
            printf("  -nacts NACTS       Number of actuators [%ld].\n",
                   nacts);
            printf("  -nbufs NBUFS       Number of frame buffers [%ld].\n",
                   nbufs);
            printf("  -perms BITS        Bitwise mask of permissions [0%o].\n",
                   perms);
            printf("  -debug             Debug mode [%s].\n",
                   (debug ? "true" : "false"));
            printf("  -h, -help, --help  Print this help.\n");
            return EXIT_SUCCESS;
        }
        if (strcmp(argv[iarg], "-orient") == 0) {
            if (iarg + 1 >= argc) {
                fprintf(stderr, "%s: missing argument for option %s\n",
                        progname, argv[iarg]);
                return EXIT_FAILURE;
Éric Thiébaut's avatar
Éric Thiébaut committed
155
            }
Éric Thiébaut's avatar
Éric Thiébaut committed
156
157
158
159
            if (sscanf(argv[iarg+1], "%iarg %c", (int*)&orient, &dummy) != 1) {
                fprintf(stderr, "%s: invalid value \"%s\" for option %s\n",
                        progname, argv[iarg+1], argv[iarg]);
                return EXIT_FAILURE;
Éric Thiébaut's avatar
Éric Thiébaut committed
160
            }
Éric Thiébaut's avatar
Éric Thiébaut committed
161
162
163
164
165
166
167
168
169
            orient &= 5;
            ++iarg;
            continue;
        }
        if (strcmp(argv[iarg], "-nacts") == 0) {
            if (iarg + 1 >= argc) {
                fprintf(stderr, "%s: missing argument for option %s\n",
                        progname, argv[iarg]);
                return EXIT_FAILURE;
170
            }
Éric Thiébaut's avatar
Éric Thiébaut committed
171
172
173
174
175
            if (sscanf(argv[iarg+1], "%ld %c", &nacts, &dummy) != 1
                || nacts < 1) {
                fprintf(stderr, "%s: invalid value \"%s\" for option %s\n",
                        progname, argv[iarg+1], argv[iarg]);
                return EXIT_FAILURE;
176
            }
Éric Thiébaut's avatar
Éric Thiébaut committed
177
178
179
180
181
182
183
184
185
186
187
188
189
            ++iarg;
            continue;
        }
        if (strcmp(argv[iarg], "-nbufs") == 0) {
            if (iarg + 1 >= argc) {
                fprintf(stderr, "%s: missing argument for option %s\n",
                        progname, argv[iarg]);
                return EXIT_FAILURE;
            }
            if (sscanf(argv[iarg+1], "%ld %c", &nbufs, &dummy) != 1
                || nbufs < 2) {
                fprintf(stderr, "%s: invalid value \"%s\" for option %s\n",
                        progname, argv[iarg+1], argv[iarg]);
Éric Thiébaut's avatar
Éric Thiébaut committed
190
191
                return EXIT_FAILURE;
            }
Éric Thiébaut's avatar
Éric Thiébaut committed
192
193
            ++iarg;
            continue;
Éric Thiébaut's avatar
Éric Thiébaut committed
194
        }
Éric Thiébaut's avatar
Éric Thiébaut committed
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
        if (strcmp(argv[iarg], "-perms") == 0) {
            if (iarg + 1 >= argc) {
                fprintf(stderr, "%s: missing argument for option %s\n",
                        progname, argv[iarg]);
                return EXIT_FAILURE;
            }
            if (sscanf(argv[iarg+1], "%iarg %c", (int*)&perms, &dummy) != 1) {
                fprintf(stderr, "%s: invalid value \"%s\" for option %s\n",
                        progname, argv[iarg+1], argv[iarg]);
                return EXIT_FAILURE;
            }
            perms &= 0777;
            ++iarg;
            continue;
        }
        if (strcmp(argv[iarg], "-debug") == 0) {
            debug = true;
            continue;
        }
        fprintf(stderr, "%s: unknown option %s\n", progname, argv[iarg]);
        return EXIT_FAILURE;
216
    }
217
218
    if (ident == NULL) {
        fprintf(stderr, "%s: missing server name\n", progname);
219
        goto bad_usage;
Éric Thiébaut's avatar
Éric Thiébaut committed
220
221
    }

222
    // Build mirror mask and layout indices.
223
    long dim = lround(2*sqrt(nacts/M_PI));
224
    msk = tao_malloc(dim*dim*sizeof(*msk));
Éric Thiébaut's avatar
Éric Thiébaut committed
225
    if (msk == NULL) {
226
227
        fprintf(stderr, "%s: cannot allocate mirror mask of size %ld×%ld "
                "for %ld actuators\n", progname, dim, dim, nacts);
Éric Thiébaut's avatar
Éric Thiébaut committed
228
229
        return EXIT_FAILURE;
    }
230
    inds = tao_malloc(dim*dim*sizeof(*inds));
Éric Thiébaut's avatar
Éric Thiébaut committed
231
232
233
234
235
    if (inds == NULL) {
        fprintf(stderr, "%s: cannot allocate array for layout indices\n",
                progname);
        return EXIT_FAILURE;
    }
236
    if (tao_layout_mask_instanciate(msk, dim, dim, nacts, inds) == NULL) {
Éric Thiébaut's avatar
Éric Thiébaut committed
237
        fprintf(stderr, "%s: failed to instantiate mirror mask\n",
238
239
240
                progname);
        return EXIT_FAILURE;
    }
241
    nacts = tao_indexed_layout_build(inds, msk, dim, dim, orient);
Éric Thiébaut's avatar
Éric Thiébaut committed
242
243
    if (nacts < 1) {
        fprintf(stderr, "%s: failed to build layout indices\n", progname);
244
        tao_report_error();
Éric Thiébaut's avatar
Éric Thiébaut committed
245
246
247
248
        return EXIT_FAILURE;
    }

    // Allocate the remote mirror instance.
249
    dm = tao_remote_mirror_create(ident, nbufs, inds, dim, dim, perms);
Éric Thiébaut's avatar
Éric Thiébaut committed
250
251
252
    if (dm == NULL) {
        fprintf(stderr, "%s: failed to create remote mirror instance\n",
                progname);
253
        tao_report_error();
Éric Thiébaut's avatar
Éric Thiébaut committed
254
255
256
257
258
        return EXIT_FAILURE;
    }

    // Run loop (on entry of the loop we own the lock on the remote mirror
    // instance).
259
260
261
262
263
264
    tao_remote_mirror_operations ops = {
        .on_send = on_send,
        .on_reset = on_reset,
        .name = ident,
        .debug = debug
    };
265
    tao_status status = tao_remote_mirror_run_loop(dm, &ops, NULL);
Éric Thiébaut's avatar
Éric Thiébaut committed
266
    if (status != TAO_OK) {
267
        tao_report_error();
Éric Thiébaut's avatar
Éric Thiébaut committed
268
269
270
271
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}