virtualcamera.c 30.9 KB
Newer Older
1
// virtualcamera.c -
2
3
4
5
6
7
8
9
//
// Program to simulate a camera in TAO infrastructure.
//
//-----------------------------------------------------------------------------
//
// This file if part of TAO real-time software licensed under the MIT license
// (https://git-cral.univ-lyon1.fr/tao/tao-rt).
//
10
// Copyright (C) 2018-2022, Éric Thiébaut.
Éric Thiébaut's avatar
Éric Thiébaut committed
11
12

#include <errno.h>
Éric Thiébaut's avatar
Éric Thiébaut committed
13
14
15
16
17
18
19
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <xpa.h>
#include <tao.h>
#include <tao-private.h>

Éric Thiébaut's avatar
Éric Thiébaut committed
20
static const char* progname = "virtualcamera";
Éric Thiébaut's avatar
Éric Thiébaut committed
21

22
23
24
// XPA routines are not thread safe, the XPA server must be run in a given
// single thread.  The following static data are only available for the server
// routines.
Éric Thiébaut's avatar
Éric Thiébaut committed
25
26
static bool debug = true;
static bool quit = false;
Éric Thiébaut's avatar
Éric Thiébaut committed
27
static tao_buffer srvbuf; // dynamic i/o buffer for sending messages to the
28
                            // client
Éric Thiébaut's avatar
Éric Thiébaut committed
29

Éric Thiébaut's avatar
Éric Thiébaut committed
30
31
#define SENSORWIDTH  640
#define SENSORHEIGHT 480
Éric Thiébaut's avatar
Éric Thiébaut committed
32

33
#if 0 // FIXME: unused
Éric Thiébaut's avatar
Éric Thiébaut committed
34
35
36
37
static const double max_rate = 2e3;
static const double min_rate = 1.0;
static const double min_exposure = 0.0;
static const double max_exposure = 1.0;
38
#endif
Éric Thiébaut's avatar
Éric Thiébaut committed
39

Éric Thiébaut's avatar
Éric Thiébaut committed
40
static tao_camera* cam = NULL;
Éric Thiébaut's avatar
Éric Thiébaut committed
41

42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
static int send_callback(
    void* send_data,
    void* call_data,
    char* command,
    char** bufptr,
    size_t* lenptr);

static int recv_callback(
    void* recv_data,
    void* call_data,
    char* command,
    char* buf,
    size_t len);

// Report TAO errors to XPA client.
static void report_error(
Éric Thiébaut's avatar
Éric Thiébaut committed
58
    tao_error** errs,
59
60
61
62
63
64
65
    XPA xpa);

//-----------------------------------------------------------------------------
// LOAD FITS FILE

// FIXME: mutex-protect these data so that image cube can be changed at
// run-time.
Éric Thiébaut's avatar
Éric Thiébaut committed
66
static tao_array* images = NULL;
Éric Thiébaut's avatar
Éric Thiébaut committed
67
68
69
70
static void* image_data = NULL;
static int   image_eltype = 0;
static long  image_width = 0;
static long  image_height = 0;
71
static long  image_stride = 0; // stride (in bytes) between successive images
Éric Thiébaut's avatar
Éric Thiébaut committed
72
73
74
static long  image_number = 0;
static long  image_counter = 0;

75
76
static void free_images(
    void)
Éric Thiébaut's avatar
Éric Thiébaut committed
77
78
79
80
81
82
83
84
85
86
87
88
89
{
    if (images != NULL) {
        image_data = NULL;
        image_eltype = 0;
        image_width = 0;
        image_height = 0;
        image_number = 0;
        tao_unreference_array(images);
        images = NULL;
    }
    reset_pre_processing();
}

90
91
92
static void load_images(
    const char* filename,
    char* extname)
Éric Thiébaut's avatar
Éric Thiébaut committed
93
{
Éric Thiébaut's avatar
Éric Thiébaut committed
94
    tao_error* errs = TAO_NO_ERRORS;
Éric Thiébaut's avatar
Éric Thiébaut committed
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
    int ndims;

    free_images();
    images = tao_load_array_from_fits_file(&errs, filename, extname);
    if (images == NULL || errs != TAO_NO_ERRORS) {
        tao_report_errors(&errs);
        exit(1);
    }
    ndims = tao_get_array_ndims(images);
    image_data   = tao_get_array_data(images);
    image_eltype = tao_get_array_eltype(images);
    image_width  = (ndims < 1 ? 1 : tao_get_array_size(images, 1));
    image_height = (ndims < 2 ? 1 : tao_get_array_size(images, 2));
    image_stride = image_width*image_height*tao_get_element_size(image_eltype);
    image_number = tao_get_array_length(images)/(image_width*image_height);
}

112
113
//-----------------------------------------------------------------------------
// GENERATE IMAGES
Éric Thiébaut's avatar
Éric Thiébaut committed
114
115
116
117
118

#if 0
static void* raw_img = NULL;
static size_t raw_size = 0;

119
120
121
122
static void* resize_image(
    long depth,
    long width,
    long height)
Éric Thiébaut's avatar
Éric Thiébaut committed
123
124
125
126
127
128
129
130
131
132
133
134
135
136
{
    size_t elsize = (depth <= 8 ? 1 : 2);
    size_t minsize = elsize*width*height;
    size_t maxsize = minsize + (minsize >> 1);
    if (raw_img == NULL || raw_size < minsize || raw_size > maxsize) {
        if (raw_img != NULL) {
            free(raw_img);
        }
        raw_img = tao_malloc(NULL, maxsize);
        raw_size = maxsize;
    }
    return raw_img;
}

137
138
139
140
141
static void* generate_image(
    long depth,
    long width,
    long height,
    uint32_t bits)
Éric Thiébaut's avatar
Éric Thiébaut committed
142
143
144
145
146
147
148
149
150
{
    void* raw = resize_image(depth, width, height);

#define GENERATE(TYPE)                                  \
    do {                                                \
        TYPE xmsk = ( bits        & 0xFF);              \
        TYPE ymsk = ((bits >>  8) & 0xFF);              \
        TYPE xoff = ((bits >> 16) & 0xFF);              \
        TYPE yoff = ((bits >> 24) & 0xFF);              \
151
        for (long y = 0; y < height; ++y) {             \
Éric Thiébaut's avatar
Éric Thiébaut committed
152
153
            TYPE* line = ((TYPE*)raw) + y*width;        \
            TYPE mask = ((y + yoff) & ymsk);            \
154
            for (long x = 0; x < width; ++x) {          \
Éric Thiébaut's avatar
Éric Thiébaut committed
155
156
157
                line[x] = mask & ((x + xoff) & xmsk);   \
            }                                           \
        }                                               \
Éric Thiébaut's avatar
Éric Thiébaut committed
158
    } while (false)
Éric Thiébaut's avatar
Éric Thiébaut committed
159
160
161
162
163
164
165
166
167
168
169
170
171
172

    if (depth <= 8) {
        GENERATE(uint8_t);
    } else {
        GENERATE(uint16_t);
    }

#undef GENERATE

    return raw;
}
#endif

#define GENERATE(FUNC, TYPE)                                    \
173
174
175
176
177
    static void FUNC(                                           \
        TYPE* dest,                                             \
        long width,                                             \
        long height,                                            \
        uint32_t bits)                                          \
Éric Thiébaut's avatar
Éric Thiébaut committed
178
179
180
181
    {                                                           \
        TYPE xmsk = 0xFF;                                       \
        TYPE xoff = ( bits        & 0xFF);                      \
        TYPE yoff = ((bits >>  8) & 0xFF);                      \
182
        for (long y = 0; y < height; ++y) {                     \
Éric Thiébaut's avatar
Éric Thiébaut committed
183
184
            TYPE* line = ((TYPE*)dest) + y*width;               \
            TYPE mask = ((y + yoff) & 0x0F) == 0x0F ? 0 : 0xFF; \
185
            for (long x = 0; x < width; ++x) {                  \
Éric Thiébaut's avatar
Éric Thiébaut committed
186
187
188
189
190
191
192
193
194
                line[x] = mask & ((x + xoff) & xmsk);           \
            }                                                   \
        }                                                       \
    }
GENERATE(generate_u8,  uint8_t);
GENERATE(generate_u16, uint16_t);

#undef GENERATE

195
196
static void produce_image(
    unsigned bits)
Éric Thiébaut's avatar
Éric Thiébaut committed
197
{
Éric Thiébaut's avatar
Éric Thiébaut committed
198
    tao_error* errs = TAO_NO_ERRORS;
Éric Thiébaut's avatar
Éric Thiébaut committed
199
    struct timespec t0, t1, dt;
Éric Thiébaut's avatar
Éric Thiébaut committed
200
    char buf[32];
Éric Thiébaut's avatar
Éric Thiébaut committed
201
    tao_shared_array* arr = NULL;
Éric Thiébaut's avatar
Éric Thiébaut committed
202
203
204
205
206

    if (tao_get_monotonic_time(&errs, &t0) != 0) {
        goto error;
    }

207
208
209
    // Lock the camera and get an array to write the captured image data.  Note
    // that, to avoid deadlocks, the camera is unlocked prior to check whether
    // the returned array is valid.
Éric Thiébaut's avatar
Éric Thiébaut committed
210
211
212
    if (tao_lock_shared_camera(&errs, cam->shared) == 0) {
        if (cam->shared->depth <= 8) {
            cam->shared->depth = 8;
Éric Thiébaut's avatar
Éric Thiébaut committed
213
            cam->shared->pixeltype = TAO_UINT8;
Éric Thiébaut's avatar
Éric Thiébaut committed
214
215
        } else {
            cam->shared->depth = 16;
Éric Thiébaut's avatar
Éric Thiébaut committed
216
            cam->shared->pixeltype = TAO_UINT16;
Éric Thiébaut's avatar
Éric Thiébaut committed
217
218
        }
        arr = tao_fetch_next_frame(&errs, cam);
219
        tao_shared_camera_unlock(&errs, cam->shared);
Éric Thiébaut's avatar
Éric Thiébaut committed
220
221
222
223
224
    }
    if (errs != TAO_NO_ERRORS) {
        goto error;
    }

225
    // Generate/process the image while the shared camera data is unlocked.
226
227
    tao_shared_array_set_timestamp(arr, t0.s, t0.ns);
    void* data = tao_shared_array_get_data(arr);
228
229
    long width = tao_get_shared_array_size(arr, 1);
    long height = tao_get_shared_array_size(arr, 2);
Éric Thiébaut's avatar
Éric Thiébaut committed
230
    if (cam->shared->pixeltype == TAO_UINT8) {
Éric Thiébaut's avatar
Éric Thiébaut committed
231
        generate_u8(data, width, height, bits);
Éric Thiébaut's avatar
Éric Thiébaut committed
232
    } else if (cam->shared->pixeltype == TAO_UINT16) {
Éric Thiébaut's avatar
Éric Thiébaut committed
233
234
235
236
237
238
239
240
        generate_u16(data, width, height, bits);
    }
    if (debug) {
        if (tao_get_monotonic_time(&errs, &t1) != 0) {
            goto error;
        }
        tao_subtract_times(&dt, &t1, &t0);
        tao_sprintf_time(buf, &dt);
241
        fprintf(stderr, "%ld×%ld image (shmid=%ld) generated in %s seconds\n",
242
                width, height, (long)tao_shared_array_get_shmid(arr), buf);
Éric Thiébaut's avatar
Éric Thiébaut committed
243
244
    }

245
    // Re-lock the shared camera data and publish image.
Éric Thiébaut's avatar
Éric Thiébaut committed
246
247
    if (tao_lock_shared_camera(&errs, cam->shared) == 0) {
        tao_publish_next_frame(&errs, cam, arr);
248
        tao_shared_camera_unlock(&errs, cam->shared);
Éric Thiébaut's avatar
Éric Thiébaut committed
249
250
251
252
253
254
    }

 error:
    tao_report_errors(&errs);
}

255
256
//-----------------------------------------------------------------------------
// ERROR MESSAGES
Éric Thiébaut's avatar
Éric Thiébaut committed
257

258
259
// The following buffer is used for storing answers or small error messages
// sent by the server.  FIXME: use srvbuf?
260
261
static char message[1024];

262
263
264
265
266
static void tao_get_error_details(
    char* buffer,
    int code,
    const char** reason,
    const char** info,
Éric Thiébaut's avatar
Éric Thiébaut committed
267
    tao_error_getter* proc)
268
269
{
    if (proc != NULL) {
270
        // Use callback to retrieve error details.
271
272
273
274
        *reason = NULL;
        *info = NULL;
        proc(code, reason, info);
    } else {
275
        // Assume a system error or a TAO error.
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
        *reason = tao_get_error_reason(code);
        *info = tao_get_error_name(code);
        if (*info != NULL) {
            if (code > 0) {
                if (strcmp(*info, "UNKNOWN_SYSTEM_ERROR") == 0) {
                    *info = NULL;
                }
            } else if (code < 0) {
                if (strcmp(*info, "UNKNOWN_ERROR") == 0) {
                    *info = NULL;
                }
            }
        }
    }
    if (*reason == NULL) {
Éric Thiébaut's avatar
Éric Thiébaut committed
291
        *reason = "Some error occurred";
292
293
    }
    if (*info == NULL) {
294
        // Use the numerical value of the error code.
295
296
297
298
299
        sprintf(buffer, "%d", code);
        *info = buffer;
    }
}

300
static void report_error(
Éric Thiébaut's avatar
Éric Thiébaut committed
301
    tao_error** errs,
302
    XPA xpa)
Éric Thiébaut's avatar
Éric Thiébaut committed
303
304
{
    const char* func;
305
306
    const char* reason;
    const char* info;
Éric Thiébaut's avatar
Éric Thiébaut committed
307
    tao_error_getter* proc;
Éric Thiébaut's avatar
Éric Thiébaut committed
308
309
    int code;
    bool flag = true;
310
    char* buf;
311
    char infobuf[20];
Éric Thiébaut's avatar
Éric Thiébaut committed
312

313
    // Write error message.
Éric Thiébaut's avatar
Éric Thiébaut committed
314
    tao_clear_buffer(&srvbuf);
315
    while (tao_pop_error(errs, &func, &code, &proc)) {
316
317
        // Append next error message to the i/o buffer.  A fatal error results
        // in case of failure here.
318
        tao_get_error_details(infobuf, code, &reason, &info, proc);
Éric Thiébaut's avatar
Éric Thiébaut committed
319
320
        if (debug) {
            fprintf(stderr, "%s %s in function `%s` [%s]\n",
321
                    (flag ? "{ERROR}" : "       "), reason, func, info);
Éric Thiébaut's avatar
Éric Thiébaut committed
322
        }
Éric Thiébaut's avatar
Éric Thiébaut committed
323
324
        tao_print_to_buffer(NULL, &srvbuf, "%s%s in function `%s` [%s]\n",
                            (flag ? "" : "; "), reason, func, info);
Éric Thiébaut's avatar
Éric Thiébaut committed
325
326
327
        flag = 0;
    }
    if (flag) {
328
        buf = tao_get_buffer_contents(&srvbuf, NULL);
Éric Thiébaut's avatar
Éric Thiébaut committed
329
    } else {
330
        // There were no errors!
331
        buf = "no errors!";
Éric Thiébaut's avatar
Éric Thiébaut committed
332
    }
333
    XPAError(xpa, buf);
Éric Thiébaut's avatar
Éric Thiébaut committed
334
335
}

336
337
338
339
340
341
342
343
344
345
// When printing error messages to a fixed size buffer, it is important to only
// print the arguments which are correct (to avoid buffer overflow).  In the
// routines below `argc` is the number of arguments to print.
static void syntax_error(
    XPA xpa,
    char* buf,
    const char* prefix,
    int argc,
    const char** argv,
    const char* suffix)
Éric Thiébaut's avatar
Éric Thiébaut committed
346
347
{
    size_t len = strlen(prefix);
348
349
350
    if (buf != prefix) {
        strcpy(buf, prefix);
    }
Éric Thiébaut's avatar
Éric Thiébaut committed
351
352
353
354
355
356
357
358
359
360
361
362
    for (int i = 0; i < argc && argv[i] != NULL; ++i) {
        if (i > 0) {
            buf[len] = ' ';
            ++len;
        }
        strcpy(buf + len, argv[i]);
        len += strlen(buf + len);
    }
    strcpy(buf + len, suffix);
    XPAError(xpa, buf);
}

363
364
365
366
367
368
static void too_few_arguments(
    XPA xpa,
    char* buf,
    const char* cmd,
    int argc,
    const char** argv)
Éric Thiébaut's avatar
Éric Thiébaut committed
369
{
370
371
    sprintf(buf, "missing argument(s) in %s `", cmd);
    syntax_error(xpa, buf, buf, argc, argv, " ...` command");
Éric Thiébaut's avatar
Éric Thiébaut committed
372
373
}

374
375
376
377
378
379
static void too_many_arguments(
    XPA xpa,
    char* buf,
    const char* cmd,
    int argc,
    const char** argv)
Éric Thiébaut's avatar
Éric Thiébaut committed
380
{
381
382
    sprintf(buf, "unexpected argument(s) after %s `", cmd);
    syntax_error(xpa, buf, buf, argc, argv, " ...` command");
Éric Thiébaut's avatar
Éric Thiébaut committed
383
384
}

385
386
387
388
389
390
static void invalid_arguments(
    XPA xpa,
    char* buf,
    const char* cmd,
    int argc,
    const char** argv)
Éric Thiébaut's avatar
Éric Thiébaut committed
391
{
392
393
    sprintf(buf, "invalid argument(s) in %s `", cmd);
    syntax_error(xpa, buf, buf, argc, argv, " ...` command");
Éric Thiébaut's avatar
Éric Thiébaut committed
394
395
}

396
//-----------------------------------------------------------------------------
Éric Thiébaut's avatar
Éric Thiébaut committed
397

398
399
400
401
402
static void start_command(int nbufs) {}
static void abort_command() {}
static void stop_command() {}
static void close_command() {}

403
404
405
#define CHECK_MIN_ARGC(nprt, nmin)                              \
    do {                                                        \
        if (argc < nmin) {                                      \
406
            too_few_arguments(xpa, message, CMD, nprt, argv);   \
407
408
            goto error;                                         \
        }                                                       \
Éric Thiébaut's avatar
Éric Thiébaut committed
409
    } while (false)
410
411
412
#define CHECK_ARGC(nprt, nmin, nmax)                            \
    do {                                                        \
        if (argc < nmin) {                                      \
413
            too_few_arguments(xpa, message, CMD, nprt, argv);   \
414
415
416
            goto error;                                         \
        }                                                       \
        if (argc > nmax) {                                      \
417
            too_many_arguments(xpa, message, CMD, nprt, argv);  \
418
419
            goto error;                                         \
        }                                                       \
Éric Thiébaut's avatar
Éric Thiébaut committed
420
    } while (false)
421
422
423

#define INVALID_ARGUMENTS(n)                            \
    do {                                                \
424
        invalid_arguments(xpa, message, CMD, n, argv);  \
425
        goto error;                                     \
Éric Thiébaut's avatar
Éric Thiébaut committed
426
    } while (false)
427

428
// Callback to answer an XPAGet request.
429
#define CMD "get"
430
431
432
433
434
435
static int send_callback(
    void* send_data,
    void* call_data,
    char* command,
    char** bufptr,
    size_t* lenptr)
Éric Thiébaut's avatar
Éric Thiébaut committed
436
{
437
    // Automatic variables.
Éric Thiébaut's avatar
Éric Thiébaut committed
438
    XPA xpa = (XPA)call_data;
Éric Thiébaut's avatar
Éric Thiébaut committed
439
    tao_error* errs = TAO_NO_ERRORS;
440
    int argc, status = 0;
Éric Thiébaut's avatar
Éric Thiébaut committed
441
    size_t len;
442
    const char** argv = NULL;
Éric Thiébaut's avatar
Éric Thiébaut committed
443
444
445
446

    if (debug) {
        fprintf(stderr, "send: %s\n", command);
    }
447
    argc = tao_split_command(&errs, &argv, command, -1);
Éric Thiébaut's avatar
Éric Thiébaut committed
448
    if (argc < 0) {
449
        goto error;
Éric Thiébaut's avatar
Éric Thiébaut committed
450
451
452
453
454
455
456
    }
    if (debug) {
        for (int i = 0; i < argc; ++i) {
            fprintf(stderr, "send: [%d] >>%s<<\n", i, argv[i]);
        }
    }

457
    // Parse command starting with an empty answer.
458
    message[0] = '\0';
459
460
461
462
463
464
    if (argc < 1) {
        goto done;
    }
    int c = argv[0][0];
    if (c == 'b' && strcmp(argv[0], "bias") == 0) {
        CHECK_ARGC(1, 1, 1);
465
        sprintf(message, "%.1f", cam->shared->bias);
466
467
    } else if (c == 'd' && strcmp(argv[0], "debug") == 0) {
        CHECK_ARGC(1, 1, 1);
468
        strcpy(message, (debug ? "on" : "off"));
Éric Thiébaut's avatar
Éric Thiébaut committed
469
    } else if (c == 'e' && strcmp(argv[0], "exposuretime") == 0) {
470
        CHECK_ARGC(1, 1, 1);
Éric Thiébaut's avatar
Éric Thiébaut committed
471
472
        sprintf(message, "%.6f", cam->shared->exposuretime);
    } else if (c == 'f' && strcmp(argv[0], "sensorheight") == 0) {
473
        CHECK_ARGC(1, 1, 1);
Éric Thiébaut's avatar
Éric Thiébaut committed
474
475
        sprintf(message, "%ld", cam->shared->sensorheight);
    } else if (c == 'f' && strcmp(argv[0], "sensorwidth") == 0) {
476
        CHECK_ARGC(1, 1, 1);
Éric Thiébaut's avatar
Éric Thiébaut committed
477
        sprintf(message, "%ld", cam->shared->sensorwidth);
478
479
    } else if (c == 'g' && strcmp(argv[0], "gamma") == 0) {
        CHECK_ARGC(1, 1, 1);
480
        sprintf(message, "%.3f", cam->shared->gamma);
481
482
    } else if (c == 'g' && strcmp(argv[0], "gain") == 0) {
        CHECK_ARGC(1, 1, 1);
483
        sprintf(message, "%.1f", cam->shared->gain);
484
485
    } else if (c == 'h' && strcmp(argv[0], "height") == 0) {
        CHECK_ARGC(1, 1, 1);
486
        sprintf(message, "%ld", cam->shared->height);
487
    } else if (c == 'p' && strcmp(argv[0], "ping") == 0) {
Éric Thiébaut's avatar
Éric Thiébaut committed
488
        struct timespec ts;
489
490
491
492
493
        CHECK_ARGC(1, 1, 1);
        if (tao_get_monotonic_time(&errs, &ts) != 0) {
            goto error;
        }
        tao_sprintf_time(message, &ts);
Éric Thiébaut's avatar
Éric Thiébaut committed
494
    } else if (c == 'r' && strcmp(argv[0], "framerate") == 0) {
495
        CHECK_ARGC(1, 1, 1);
Éric Thiébaut's avatar
Éric Thiébaut committed
496
        sprintf(message, "%.3f", cam->shared->framerate);
497
498
    } else if (c == 'r' && strcmp(argv[0], "roi") == 0) {
        CHECK_ARGC(1, 1, 1);
499
        sprintf(message, "%ld %ld %ld %ld",
500
501
502
503
                cam->shared->xoff, cam->shared->yoff,
                cam->shared->width, cam->shared->height);
    } else if (c == 's' && strcmp(argv[0], "shmid") == 0) {
        CHECK_ARGC(1, 1, 1);
504
        sprintf(message, "%ld", (long)tao_shared_camera_get_shmid(cam->shared));
505
506
    } else if (c == 's' && strcmp(argv[0], "state") == 0) {
        CHECK_ARGC(1, 1, 1);
507
        sprintf(message, "%d", cam->shared->state);
508
509
    } else if (c == 'w' && strcmp(argv[0], "width") == 0) {
        CHECK_ARGC(1, 1, 1);
510
        sprintf(message, "%ld", cam->shared->width);
511
512
    } else if (c == 'x' && strcmp(argv[0], "xoff") == 0) {
        CHECK_ARGC(1, 1, 1);
513
        sprintf(message, "%ld", cam->shared->xoff);
514
515
    } else if (c == 'y' && strcmp(argv[0], "yoff") == 0) {
        CHECK_ARGC(1, 1, 1);
516
        sprintf(message, "%ld", cam->shared->yoff);
517
518
519
520
    } else {
        XPAError(xpa, "unknown parameter for get `...` command");
        goto error;
    }
Éric Thiébaut's avatar
Éric Thiébaut committed
521

522
 done:
523
    len = strlen(message);
524
    if (len > 0) {
525
526
        // Append a newline for more readable output when xapset/xpaget are
        // used from the command line.
527
528
529
530
        message[len] = '\n';
        message[len+1] = '\0';
    }
    *bufptr = message;
531
532
    *lenptr = len + 1;
 cleanup:
533
    if (errs != TAO_NO_ERRORS) {
534
        // FIXME: This should not happen, at least print a warning.
535
536
        tao_discard_errors(&errs);
    }
537
538
539
540
541
542
    if (argv != NULL) {
        free(argv);
    }
    return status;

 error:
543
544
545
    if (errs != TAO_NO_ERRORS) {
        report_error(&errs, xpa);
    }
546
547
548
549
550
    status = -1;
    goto cleanup;
}


551
// Callback to answer an XPASet request.
552
553
#undef CMD
#define CMD "set"
554
555
556
557
558
559
static int recv_callback(
    void* recv_data,
    void* call_data,
    char* command,
    char* buf,
    size_t len)
560
561
{
    XPA xpa = (XPA)call_data;
Éric Thiébaut's avatar
Éric Thiébaut committed
562
    tao_error* errs = TAO_NO_ERRORS;
563
564
    int argc, status = 0;
    const char** argv = NULL;
565
566
567
568

    if (debug) {
        fprintf(stderr, "recv: %s [%ld byte(s)]\n", command, (long)len);
    }
569
    argc = tao_split_command(&errs, &argv, command, -1);
570
    if (argc < 0) {
571
572
        report_error(&errs, xpa);
        goto error;
573
574
575
576
577
578
579
    }
    if (debug) {
        for (int i = 0; i < argc; ++i) {
            fprintf(stderr, "recv: [%d] >>%s<<\n", i, argv[i]);
        }
    }

580
    // Commands take no data, check this.
581
582
583
584
    if (len > 0) {
        XPAError(xpa, "expecting no data");
        goto error;
    }
Éric Thiébaut's avatar
Éric Thiébaut committed
585

586
    // Parse command.
Éric Thiébaut's avatar
Éric Thiébaut committed
587
588
589
590
591
592
593
594
595
596
597
598
599
    if (argc < 1) {
        goto done;
    }
    int c = argv[0][0];
    if (c == 'a' && strcmp(argv[0], "abort") == 0) {
        CHECK_ARGC(1, 1, 1);
        if (cam->shared->state == 2) {
            abort_command();
            cam->shared->state = 1;
        } else {
            XPAError(xpa, "no acquisition is running");
            goto error;
        }
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
    } else if (c == 'b' && strcmp(argv[0], "bias") == 0) {
        double bias;
        CHECK_ARGC(1, 2, 2);
        if (tao_parse_double(argv[1], &bias) != 0) {
            XPAError(xpa, "bad value for `bias`");
            goto error;
        }
        if (cam->shared->state >= 2) {
            XPAError(xpa, "cannot change `bias` during an acquisition");
            goto error;
        }
        cam->shared->bias = bias;
    } else if (c == 'd' && strcmp(argv[0], "debug") == 0) {
        CHECK_ARGC(1, 2, 2);
        if (strcmp(argv[1], "on") == 0) {
Éric Thiébaut's avatar
Éric Thiébaut committed
615
            debug = true;
616
        } else if (strcmp(argv[1], "off") == 0) {
Éric Thiébaut's avatar
Éric Thiébaut committed
617
            debug = false;
Éric Thiébaut's avatar
Éric Thiébaut committed
618
        } else {
619
620
621
622
623
624
625
            INVALID_ARGUMENTS(1);
        }
    } else if (c == 'g' && strcmp(argv[0], "gain") == 0) {
        double gain;
        CHECK_ARGC(1, 2, 2);
        if (tao_parse_double(argv[1], &gain) != 0 || gain <= 0) {
            XPAError(xpa, "bad value for `gain`");
Éric Thiébaut's avatar
Éric Thiébaut committed
626
627
            goto error;
        }
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
        if (cam->shared->state >= 2) {
            XPAError(xpa, "cannot change `gain` during an acquisition");
            goto error;
        }
        cam->shared->gain = gain;
    } else if (c == 'g' && strcmp(argv[0], "gamma") == 0) {
        double gamma;
        CHECK_ARGC(1, 2, 2);
        if (tao_parse_double(argv[1], &gamma) != 0 || gamma <= 0 ||
            gamma > 2) {
            XPAError(xpa, "bad value for `gamma`");
            goto error;
        }
        if (cam->shared->state >= 2) {
            XPAError(xpa, "cannot change `gamma` during an acquisition");
            goto error;
        }
        cam->shared->gamma = gamma;
Éric Thiébaut's avatar
Éric Thiébaut committed
646
647
648
649
650
651
652
653
654
655
656
657
658
    } else if (c == 'p' && strcmp(argv[0], "produce") == 0) {
        CHECK_ARGC(1, 1, 1);
        produce_image(0x3256ae07);
    } else if (c == 'q' && strcmp(argv[0], "quit") == 0) {
        CHECK_ARGC(1, 1, 1);
        if (cam->shared->state == 2) {
            abort_command();
            cam->shared->state = 1;
        }
        if (cam->shared->state == 1) {
            close_command();
            cam->shared->state = 0;
        }
Éric Thiébaut's avatar
Éric Thiébaut committed
659
        quit = true;
660
    } else if (c == 'r' && strcmp(argv[0], "roi") == 0) {
661
        long xoff, yoff, width, height;
662
        CHECK_ARGC(1, 5, 5);
663
        if (tao_parse_long(argv[1], &xoff, 10) != 0 || xoff < 0 ||
Éric Thiébaut's avatar
Éric Thiébaut committed
664
            xoff >= SENSORWIDTH) {
665
666
667
            XPAError(xpa, "bad value for `xoff` in set `roi ...` command");
            goto error;
        }
668
        if (tao_parse_long(argv[2], &yoff, 10) != 0 || yoff < 0 ||
Éric Thiébaut's avatar
Éric Thiébaut committed
669
            yoff >= SENSORHEIGHT) {
670
671
672
            XPAError(xpa, "bad value for `yoff` in set `roi ...` command");
            goto error;
        }
673
        if (tao_parse_long(argv[3], &width, 10) != 0 || width < 1 ||
Éric Thiébaut's avatar
Éric Thiébaut committed
674
            width > SENSORWIDTH) {
675
676
677
            XPAError(xpa, "bad value for `width` in set `roi ...` command");
            goto error;
        }
678
        if (tao_parse_long(argv[4], &height, 10) != 0 || height < 1 ||
Éric Thiébaut's avatar
Éric Thiébaut committed
679
            height > SENSORHEIGHT) {
680
681
682
            XPAError(xpa, "bad value for `height` in set `roi ...` command");
            goto error;
        }
Éric Thiébaut's avatar
Éric Thiébaut committed
683
        if (xoff + width > SENSORWIDTH) {
684
685
686
687
            XPAError(xpa, "`xoff + width` too large in set `roi ...` "
                     "command");
            goto error;
        }
Éric Thiébaut's avatar
Éric Thiébaut committed
688
        if (yoff + height > SENSORHEIGHT) {
689
690
691
692
693
694
            XPAError(xpa, "`yoff + height` too large in set `roi ...` "
                     "command");
            goto error;
        }
        if (cam->shared->state >= 2) {
            XPAError(xpa, "cannot change ROI during an acquisition");
Éric Thiébaut's avatar
Éric Thiébaut committed
695
696
            goto error;
        }
697
698
699
700
        cam->shared->xoff = xoff;
        cam->shared->yoff = yoff;
        cam->shared->width = width;
        cam->shared->height = height;
Éric Thiébaut's avatar
Éric Thiébaut committed
701
702
703
704
    } else if (c == 's' && strcmp(argv[0], "start") == 0) {
        int nbufs;
        CHECK_ARGC(1, 1, 2);
        if (argc == 2) {
705
            if (tao_parse_int(argv[1], &nbufs, 10) != 0 || nbufs <= 0) {
Éric Thiébaut's avatar
Éric Thiébaut committed
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
                XPAError(xpa, "invalid number of buffers");
                goto error;
            }
        } else {
            nbufs = 4;
        }
        if (cam->shared->state == 0 || cam->shared->state == 1) {
            start_command(nbufs);
            cam->shared->state = 2;
        } else if (cam->shared->state >= 2) {
            XPAError(xpa, "acquisition is already running");
            goto error;
        }
    } else if (c == 's' && strcmp(argv[0], "stop") == 0) {
        CHECK_ARGC(1, 1, 1);
        if (cam->shared->state == 2) {
            stop_command();
            cam->shared->state = 1;
        } else {
            XPAError(xpa, "no acquisition is running");
            goto error;
        }
    } else {
729
        XPAError(xpa, "unknown set command or parameter");
Éric Thiébaut's avatar
Éric Thiébaut committed
730
731
732
733
        goto error;
    }

 done:
734
    if (errs != TAO_NO_ERRORS) {
735
        // FIXME: This should not happen, at least print a warning.
736
737
        tao_discard_errors(&errs);
    }
Éric Thiébaut's avatar
Éric Thiébaut committed
738
739
740
741
742
743
    if (argv != NULL) {
        free(argv);
    }
    return status;

 error:
744
745
746
    if (errs != TAO_NO_ERRORS) {
        report_error(&errs, xpa);
    }
Éric Thiébaut's avatar
Éric Thiébaut committed
747
    status = -1;
748
    goto done;
Éric Thiébaut's avatar
Éric Thiébaut committed
749
750
}

751
752
753
754
755
#undef CHECK_MIN_ARGC
#undef CHECK_ARGC
#undef INVALID_ARGUMENTS
#undef CMD

756
757
//-----------------------------------------------------------------------------
// IMAGE PRODUCER THREAD
Éric Thiébaut's avatar
Éric Thiébaut committed
758
759

#define GIGA 1000000000
760
761
static void* run_acquisition(
    void* arg)
Éric Thiébaut's avatar
Éric Thiébaut committed
762
{
Éric Thiébaut's avatar
Éric Thiébaut committed
763
    tao_error* errs = TAO_NO_ERRORS;
764
    tao_time_member nsec;
Éric Thiébaut's avatar
Éric Thiébaut committed
765
    struct timespec dt, t1, t0;
Éric Thiébaut's avatar
Éric Thiébaut committed
766
767
768
    tao_array* arr;
    tao_camera* cam = (tao_camera*)arg;
    tao_shared_camera* shared = cam->shared;
Éric Thiébaut's avatar
Éric Thiébaut committed
769
770
    int status;

771
    // Get starting time.
Éric Thiébaut's avatar
Éric Thiébaut committed
772
773
774
775
776
    if (tao_get_monotonic_time(&errs, &t1) != 0) {
        goto error;
    }

    while (! quit) {
777
        // Time interval (in nanoseconds) between frames.
Éric Thiébaut's avatar
Éric Thiébaut committed
778
779
780
781
        if (tao_lock_shared_camera(&errs, shared) != 0) {
            goto error;
        }
        nsec = lround(1E9/shared->framerate);
782
        if (tao_shared_camera_unlock(&errs, shared) != 0) {
Éric Thiébaut's avatar
Éric Thiébaut committed
783
784
785
            goto error;
        }

786
        // Increment image counter and time T1 of next frame.
Éric Thiébaut's avatar
Éric Thiébaut committed
787
788
789
790
791
792
793
        ++image_counter;
        t1.nsec += nsec;
        if (t1.nsec >= GIGA) {
            t1.sec += t1.nsec/GIGA;
            t1.nsec %= GIGA;
        }

794
        // Get current time T0.
Éric Thiébaut's avatar
Éric Thiébaut committed
795
796
797
798
        if (tao_get_monotonic_time(&errs, &t0) != 0) {
            goto error;
        }

799
        // Compute interval of time to sleep.
Éric Thiébaut's avatar
Éric Thiébaut committed
800
801
802
803
804
805
806
807
808
809
810
        dt.sec = t1.sec - t0.sec;
        dt.nsec = t1.nsec - t0.nsec;
        if (dt.nsec < 0) {
            dt.sec -= 1;
            dt.nsec += GIGA;
        }
        if (dt.sec < 0) {
            ++image_losts;
            continue;
        }

811
        // Sleep until next frame is ready.
Éric Thiébaut's avatar
Éric Thiébaut committed
812
813
814
815
816
817
        if (nanosleep(&dt, NULL) != 0) {
            int code = errno;
            if (code == EINTR) {
                ++image_losts;
                continue;
            } else {
818
                tao_store_error("nanosleep", code);
Éric Thiébaut's avatar
Éric Thiébaut committed
819
820
821
822
                goto error;
            }
        }

823
824
825
826
        // Fetch a shared array for storing the next image. (FIXME: This could
        // be done before sleeping and skipping lost images to reduce the
        // latency and maintain the consistency of the circular list of
        // frames.)
Éric Thiébaut's avatar
Éric Thiébaut committed
827
828
829
830
831
832
        if (tao_lock_shared_camera(&errs, shared) != 0) {
            goto error;
        }
        shared->pixeltype = image_eltype;
        shared->width = image_width;
        shared->height = image_height;
833
        shared->weighted = 0; // FIXME: implement weighted images
Éric Thiébaut's avatar
Éric Thiébaut committed
834
        arr = tao_fetch_next_frame(&errs, cam);
835
        if (tao_shared_camera_unlock(&errs, shared) != 0) {
Éric Thiébaut's avatar
Éric Thiébaut committed
836
837
838
839
840
841
842
843
844
            goto error;
        }
        if (arr == NULL) {
            goto error;
        }
        arr->counter = image_counter;
        arr->ts_sec = t1.sec;
        arr->ts_nsec = t1.nsec;

845
        // Copy new image in shared array.
Éric Thiébaut's avatar
Éric Thiébaut committed
846
847
848
849
850
851
852
853
854
855
856
857
858
        long dims[2], lens[2];
        long offset = (image_counter%image_number)*image_stride;
        dim[0] = image_width;
        dim[1] = image_height;
        lens[0] = image_width;
        lens[1] = image_height;
        if (tao_copy(&errs,
                     (char*)arr + arr->offset, arr->eltype, arr->dims, NULL,
                     (char*)image_data + offset, image_eltype, dims, NULL,
                     lens, 2) != 0) {
            goto error;
        }

859
        // Re-lock the shared camera data and publish image.
Éric Thiébaut's avatar
Éric Thiébaut committed
860
861
862
863
        if (tao_lock_shared_camera(&errs, shared) != 0) {
            goto error;
        }
        status = tao_publish_next_frame(&errs, cam, arr);
864
        if (tao_shared_camera_unlock(&errs, shared) != 0) {
Éric Thiébaut's avatar
Éric Thiébaut committed
865
866
867
868
869
870
871
872
873
874
875
876
877
878
            goto error;
        }
        if (status != 0) {
            goto error;
        }
    }
    tao_discard_errors(&errs);
    return NULL;

 error:
    tao_report_errors(&errs);
    return NULL;
}

879
880
//-----------------------------------------------------------------------------
// PRE-PROCESSING
Éric Thiébaut's avatar
Éric Thiébaut committed
881

882
883
static void reset_pre_processing(
    void)
Éric Thiébaut's avatar
Éric Thiébaut committed
884
885
886
{
}

887
888
static void allocate_pre_processing(
    void)
Éric Thiébaut's avatar
Éric Thiébaut committed
889
890
891
{
}

892
//-----------------------------------------------------------------------------
Éric Thiébaut's avatar
Éric Thiébaut committed
893

894
895
896
int main(
    int argc,
    char* argv[])
Éric Thiébaut's avatar
Éric Thiébaut committed
897
898
899
900
901
902
903
904
{
    XPA srv;
    char* serverclass = "TAO";
    char* servername = "virtualcamera1";
    char* send_mode = "acl=true,freebuf=false";
    char* recv_mode = "";
    void* send_data = NULL;
    void* recv_data = NULL;
Éric Thiébaut's avatar
Éric Thiébaut committed
905
    tao_error* errs = NULL;
906
907
    int nframes = 10; // maximum number of frames to memorize
    int msec = 100; // milliseconds
Éric Thiébaut's avatar
Éric Thiébaut committed
908

909
    // Initialize resources with default settings.
Éric Thiébaut's avatar
Éric Thiébaut committed
910
911
912
913
914
915
    tao_initialize_static_buffer(&srvbuf);
    cam = tao_create_camera(&errs, nframes, 0660);
    if (cam == NULL) {
        tao_report_errors(&errs);
        exit(1);
    }
916
    if (debug) {
Éric Thiébaut's avatar
Éric Thiébaut committed
917
        fprintf(stderr, "XPA: %s:%s\n", serverclass, servername);
918
        fprintf(stderr, "shmid: %ld\n",
919
                (long)tao_shared_camera_get_shmid(cam->shared));
920
    }
Éric Thiébaut's avatar
Éric Thiébaut committed
921
922
923
924
925
926
    cam->shared->state = 0;
    cam->shared->depth = 8;
    cam->shared->xoff = 0;
    cam->shared->yoff = 0;
    cam->shared->width = 384;
    cam->shared->height = 288;
Éric Thiébaut's avatar
Éric Thiébaut committed
927
928
929
930
931
    cam->shared->sensorwidth = SENSORWIDTH;
    cam->shared->sensorheight = SENSORHEIGHT;
    cam->shared->exposuretime = 0.001;
    cam->shared->framerate = 50.0;
    cam->shared->gain = 100.0;
Éric Thiébaut's avatar
Éric Thiébaut committed
932
933
    cam->shared->bias = 500.0;
    cam->shared->gamma = 1.0;
Éric Thiébaut's avatar
Éric Thiébaut committed
934

935
    // Parse the command line options.
Éric Thiébaut's avatar
Éric Thiébaut committed
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
    for (int i = 1; i < argc; ++i) {
        const char* opt;
        const char* arg = argv[i];
        if (arg[0] != '-') {
            break;
        }
        if (arg[1] == '-') {
            if (arg[2] == '\0') {
                ++i;
                break;
            }
            opt = arg + 2;
        } else {
            opt = arg + 1;
        }
        if (strcmp(opt, "help") == 0) {
            if (pass != 2) {
                continue;
            }
            fprintf(stderr, "Usage: %s [OPTIONS] [--] DATAFILE\n",
                    progname);
            fprintf(stderr,
                    "Simulate a camera acquiring the images in "
                    "DATAFILE (a FITS file).\n");
            fprintf(stderr, "Options:\n");
            fprintf(stderr, "  -serverclass CLASS           "
                    "XPA server class [%s].\n", serverclass);
            fprintf(stderr, "  -servername NAME             "
                    "XPA server name [%s].\n", servername);
            fprintf(stderr, "  -framerate FPS                    "
                    "Frames per second [%g Hz].\n", cfg.framerate);
            fprintf(stderr, "  -quiet                       "
                    "Quiet (non-verbose) mode.\n");
            fprintf(stderr, "  -help                        "
                    "Print this help.\n");
            exit(0);
        } else if (strcmp(opt, "framerate") == 0) {
            if (++i >= argc) {
                missing_argument(opt);
            }
            if (pass != 2) {
                continue;
            }
            if (sscanf(argv[i], " %lf %c", &cfg.framerate, &c) != 1 ||
                cfg.framerate <= 0 || cfg.framerate > 100000) {
                invalid_argument(opt);
            }
        } else if (strcmp(opt, "quiet") == 0) {
            quiet = true;
        } else {
            fatal("unknown option `%s`, try `-help` for a short help",
                  argv[i]);
        }
    }
    if (i < argc) {
        fatal("too many arguments");
    }








For faster browsing, not all history is shown. View entire blame