realtime.c 23 KB
Newer Older
1
2
3
4
5

#ifdef REAL_TIME

#include "realtime.h"
#include "time_functions.h"
Timothee Habra's avatar
Timothee Habra committed
6
//#include "cmake_config.h"
7
#include "useful_functions.h"
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#ifdef SDL
#include "plot_sdl.h"
#endif

#ifdef JAVA
#include "java_functions.h"
#include "visu_past.h"
#endif

// -- Macros -- //

#define USEC_TO_SEC 1.0e-6         ///< factor from us to s
#define DEFAULT_DELTA_T_USEC 1e5   ///< default time between 2 refresh moments (in us)
#define NB_REAL_TIME_CONSTRAINTS 2 ///< number of real-time constraints
#define REAL_TIME_SPEED_PERIOD_USEC 25e4 ///< real time speed factor computation time [s]

/*! \brief initialize the real-time structures
30
 *
31
 * \param[in,out] mbs_data Robotran main structure
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
32
 * \param[in] t0 initial time [s]
33
 * \param[in] tf final time [s]
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
34
 * \param[in] dt0 intitial time step [s]
35
 */
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
36
void mbs_realtime_init(MbsData* mbs_data, double t0, double tf, double dt0)
37
38
39
40
41
{
    // -- variables declaration -- //
    Simu_realtime *realtime;
    Realtime_option *options;

42
    options = init_Realtime_option(mbs_data);
43

44
    // options determined by the simulation
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
45
    options->t0   = t0;
46
47
48
49
    options->tf   = tf;
    options->dt0  = dt0;
    options->nb_q = mbs_data->njoint;

50
    mbs_data->fct.user.user_realtime_options(options);
51
52
53
54

    check_user_realtime_options(options);
    compute_buffer_size(options);

55
    realtime = init_realtime(mbs_data, options);
56
57
58
59
60

    realtime->options = options;

    // initial plot
    #ifdef SDL
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
61
62
63
64
    if (options->flag_plot)
    {
        plot_screen_sdl(realtime, options->t0, 2);
    }
65
66
    #endif

67
    mbs_data->realtime = realtime;
68
69
70
}

/*! \brief update save vectors for past investigations (graphs and Java visualization)
71
 *
72
73
74
75
76
77
78
 * \param[in,out] realtime real-time structure
 * \param[in] tsim current simulation time [s]
 */
void mbs_realtime_update(Simu_realtime *realtime, double tsim)
{
    // update simulation vectors save for the plot
    #ifdef SDL
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
79
80
81
82
    if (realtime->flag_plot)
    {
        update_full_vectors(realtime, tsim);
    }
83
84
85
86
    #endif

    // update simulation vectors save for the Java visualization
    #ifdef JAVA
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
87
88
89
90
    if (realtime->flag_visu)
    {
        update_past_visu(realtime, tsim);
    }
91
92
93
94
    #endif
}

/*! \brief one loop iteration of the real-time process
95
 *
96
97
98
99
100
 * \param[in,out] realtime real-time structure
 * \param[in] tsim current simulation time [s]
 */
void mbs_realtime_loop(Simu_realtime *realtime, double tsim)
{
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
101
    // variables declaration
102
    int i;
103
    int cur_t_usec;
104
105
106
    int init_t_sec, init_t_usec;
    int speed_last_t_usec;
    int speed_new_t_usec;
107

108
109
110
111
    double speed_new_tsim;
    double speed_last_tsim;

    Realtime_constraint **constraints;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
112
    MbsData* mbs_data;
113

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
114
    // variables initialization
115
116
117
118
119
120
    init_t_sec  = realtime->init_t_sec;
    init_t_usec = realtime->init_t_usec;

    speed_last_t_usec = realtime->speed_last_t_usec;
    speed_last_tsim   = realtime->speed_last_tsim;

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
121
122
    mbs_data = realtime->ext->mbs_data;

123
124
125
126
127
128
129
130
131
    cur_t_usec = t_usec(init_t_sec, init_t_usec);

    constraints = realtime->constraints;

    realtime->tsim = tsim;

    // -- Update SDL vectors to plot -- //

    #ifdef SDL
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
132
133
134
    if (realtime->flag_plot)
    {
        // assign values for the plot
135
        mbs_data->fct.user.user_realtime_plot(mbs_data);
136

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
137
138
        // check maximal values for the plot
        update_y_min_max(realtime);
139
    }
140
141
142
    #endif

    #ifdef JAVA
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
143
144
145
146
    if (realtime->flag_visu)
    {
        java_time_visu_past(realtime, tsim);
    }
147
148
149
150
151
152
153
154
155
156
157
158
    #endif


    // -- Real time gestion -- //

    // update real-time constraints
    for (i=0; i<NB_REAL_TIME_CONSTRAINTS; i++)
    {
        if (tsim >= constraints[i]->next_tsim)
        {
            // plot screen sdl
            #ifdef SDL
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
159
            if (realtime->flag_plot)
160
            {
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
161
162
163
164
165
                if (!i)
                {
                    update_plot_vectors(realtime, tsim);
                    plot_screen_sdl(realtime, tsim, 0);
                }
166
167
168
169
170
            }
            #endif

            // Java visualization
            #ifdef JAVA
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
171
            if (realtime->flag_visu)
172
            {
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
173
174
175
176
                if (i == 1)
                {
                    update_java(realtime);
                }
177
178
            }
            #endif
179

180
181
182
183
184
185
186
            // new real-time constraints
            update_realtime_constraint(constraints[i], realtime->simu_speed_flag);
        }
    }

    // handle events (coming from the keyboard...)
    #ifdef SDL
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
187
188
189
    if (realtime->flag_plot)
    {
        events_simu(realtime, tsim);
190
    }
191
192
193
194
195
196
197
198
199
200
201
202
203
    #endif

    // gate locked: waiting time
    if (tsim >= realtime->next_tsim_gate)
    {
        // loop in order to wait to respect the real-time constraints
        while (realtime->next_t_usec_gate > cur_t_usec)
        {
            // update real time
            cur_t_usec = t_usec(init_t_sec, init_t_usec);

            // handle events (coming from the keyboard...)
            #ifdef SDL
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
204
205
206
207
            if (realtime->flag_plot)
            {
                events_simu(realtime, tsim);
            }
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
            #endif
        }

        // in case there is no additional constraint
        if (realtime->no_additional_constraint)
        {
            update_realtime_constraint(realtime->constraints[0], realtime->simu_speed_flag);
        }

        // update real-time strcuture and related variables
        update_simu_realtime(realtime);
    }

    // computes the real time speed factor of the simulation (every REAL_TIME_SPEED_PERIOD_USEC s)
    if (cur_t_usec - speed_last_t_usec > REAL_TIME_SPEED_PERIOD_USEC)
    {
        speed_new_t_usec = cur_t_usec;
        speed_new_tsim   = tsim;

        realtime->real_simu_speed_factor = (speed_new_tsim - speed_last_tsim) / (1.0e-6 * (speed_new_t_usec - speed_last_t_usec));

        // change the plot
        #ifdef SDL
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
231
232
233
234
        if (realtime->flag_plot)
        {
            set_bottom_flag(realtime);
        }
235
        #endif
236

237
238
239
240
241
242
        realtime->speed_last_t_usec = speed_new_t_usec;
        realtime->speed_last_tsim   = speed_new_tsim;
    }
}

/*! \brief release memory used for the real-time related functions
243
 *
244
245
246
247
248
249
 * \param[out] realtime real-time structure to release memory
 */
void mbs_realtime_finish(Simu_realtime *realtime)
{
    // SDL window
    #ifdef SDL
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
250
251
252
253
    if (realtime->flag_plot)
    {
        last_break_gestion(realtime, realtime->tsim);
    }
254
255
256
257
258
259
260
    #endif

    // Real-time constraints
    free_simu_realtime(realtime);
}

/*! \brief generates a real-time options structures with the default values
261
 *
262
263
 * \return requested real-time options structure
 */
264
Realtime_option* init_Realtime_option(MbsData *mbs_data)
265
266
267
268
269
270
{
    Realtime_option *options;

    options = (Realtime_option*) malloc(sizeof(Realtime_option));

    // simulation
271
272
273
    options->t0  = 0.0; ///< initial simulation time [s]
    options->tf  = 1.0; ///< final simulation time [s]
    options->dt0 = 1.0e-3; ///< simulation time step [s]
274
275
276
277
278
279
280

    // general
    options->init_break  = 0; ///< 1 to start with a break, 0 otherwise
    options->final_break = 0; ///< 1 to finish with a break, 0 otherwise

    options->buffer_size = -1; ///< size of the buffer for java and sdl

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
281
282
    options->init_speed_factor = 1.0; ///< initial speed factor

283
    // SDL plots and interactions
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
284
285
286
    options->flag_plot = 1; ///< 1 if SDL plots features activated, 0 otherwise

    options->curve_width   = 3;   ///< plot curve width in number of pixels
287
288
289
290
    options->screen_width  = 660; ///< plot screen width in number of pixels
    options->screen_height = 520; ///< plot screen height in number of pixels

    options->max_nb_curves  = 12; ///< maximum number of curves plotted
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
291
    options->max_nb_legends = 6;  ///< maximum number of curves legends indicated
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312

    options->y_min_init = -1.0; ///< initial lower bound for the y values plotted
    options->y_max_init =  1.0; ///< initial upper bound for the y values plotted

    // colors of the curves
    options->color_tab[0]  = BLUE_SDL;
    options->color_tab[1]  = RED_SDL;
    options->color_tab[2]  = DARK_GREEN_SDL;
    options->color_tab[3]  = PURPLE_SDL;
    options->color_tab[4]  = ORANGE_SDL;
    options->color_tab[5]  = LIGHT_BLUE_SDL;
    options->color_tab[6]  = PINK_SDL;
    options->color_tab[7]  = TURQUOISE_SDL;
    options->color_tab[8]  = LIGHT_GREEN_SDL;
    options->color_tab[9]  = DARK_YELLOW_SDL;
    options->color_tab[10] = YELLOW_SDL;
    options->color_tab[11] = GREEN_SDL;

    options->fqc_screen = 30.0; ///< frequence of the SDL plots refreshment [Hz]

    // Java visualization
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
313
314
    options->flag_visu = 1; ///< 1 if Java visualization features activated, 0 otherwise

315
316
317
    options->nb_q = 1; ///< number of joints in the .mbs used for visualization

    // path and file name fot the .mbs file used for Java visualization
318
    options->mbs_file = mbs_data->mbs_filename;
319
320
321

    options->start_viewpoint  = 0; ///< initial visu viewpoint ID
    options->nb_viewpoints    = 1; ///< number of viewpoints (defined in .mbs)
322

323
324
325
326
327
328
    options->fqc_visu   = 30.0; ///< frequence of the java visualization refreshment [Hz]

    return options;
}

/*! \brief release memory for the options structure
329
 *
330
331
332
333
334
335
336
337
 * \param[out] options options structure
 */
void free_Realtime_option(Realtime_option *options)
{
    free(options);
}

/*! \brief check that the modifications performed by the user on the options are correct
338
 *
339
340
341
342
343
344
345
 * \param[in,out] options options structure
 */
void check_user_realtime_options(Realtime_option *options)
{
    // errors generated
    if (options->t0 >= options->tf)
    {
346
        printf("\nReal-time options error: t0 (%f) >= tf (%f).\n", options->t0, options->tf);
347
348
        exit(EXIT_FAILURE);
    }
349

350
    if (options->dt0 <= 0.0)
351
    {
352
        printf("\nReal-time options error: dt0 (%f) is not strictly positive.\n", options->dt0);
353
354
355
356
357
        exit(EXIT_FAILURE);
    }

    if (options->y_min_init >= options->y_max_init)
    {
358
        printf("\nReal-time options error: y_min_init (%f) >= y_max_init (%f).\n", options->y_min_init, options->y_max_init);
359
360
361
362
363
        exit(EXIT_FAILURE);
    }

    if (options->nb_q < 0)
    {
364
        printf("\nReal-time options error: nb_q (%d) negative.\n", options->nb_q);
365
366
367
368
        exit(EXIT_FAILURE);
    }

    // warnings generated
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
369
370
    if (options->init_speed_factor <= 0.0)
    {
371
        printf("\nReal-time options warning: init_speed_factor (%f) <= 0.0; set to 1.0.\n", options->init_speed_factor);
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
372
373
374
        options->init_speed_factor = 1.0;
    }

375
376
    if (options->start_viewpoint < -1)
    {
377
        printf("\nReal-time options warning: start_viewpoint (%d) < -1; set to 0.\n", options->start_viewpoint);
378
379
380
381
382
        options->start_viewpoint = 0;
    }

    if (options->nb_viewpoints < 0)
    {
383
        printf("\nReal-time options warning: nb_viewpoints (%d) < 0; set to 1.\n", options->nb_viewpoints);
384
385
386
387
388
        options->nb_viewpoints = 1;
    }

    if (options->curve_width < 1)
    {
389
        printf("\nReal-time options warning: curve_width (%d) < 1; set to 3.\n", options->curve_width);
390
391
392
        options->curve_width = 3;
    }

393
    if (options->screen_width < 200)
394
    {
395
        printf("\nReal-time options warning: screen_width (%d) < 200; set to 660.\n", options->screen_width);
396
397
398
        options->screen_width = 660;
    }

399
    if (options->screen_height < 200)
400
    {
401
        printf("\nReal-time options warning: screen_height (%d) < 200; set to 520.\n", options->screen_height);
402
403
404
405
406
        options->screen_height = 520;
    }

    if (options->max_nb_curves < 0)
    {
407
        printf("\nReal-time options warning: max_nb_curves (%d) < 0; set to 12.\n", options->max_nb_curves);
408
409
410
411
412
        options->max_nb_curves = 12;
    }

    if (options->max_nb_legends > options->max_nb_curves)
    {
413
        printf("\nReal-time options warning: max_nb_legends (%d) > max_nb_curves (%d); set to %d.\n", options->max_nb_legends, options->max_nb_curves, options->max_nb_curves);
414
415
416
417
418
        options->max_nb_legends = options->max_nb_curves;
    }

    if (options->fqc_visu <= 0.0)
    {
419
        printf("\nReal-time options warning: fqc_visu (%f) <= 0.0; set to 30.0.\n", options->fqc_visu);
420
421
422
423
424
        options->fqc_visu = 30.0;
    }

    if (options->fqc_screen <= 0.0)
    {
425
        printf("\nReal-time options warning: fqc_screen (%f) <= 0.0; set to 30.0.\n", options->fqc_screen);
426
427
428
429
430
        options->fqc_screen = 30.0;
    }
}

/*! \brief automatically compute the buffer size
431
 *
432
433
434
435
436
437
 * \param[in,out] options options structure
 */
void compute_buffer_size(Realtime_option *options)
{
    if (options->buffer_size <= 0)
    {
438
        options->buffer_size = ((options->tf - options->t0) / options->dt0) + 1; // size of the SDL buffer [-], if possible, equal (or higher) to the number of time steps
439
440
441
442
    }
}

/*! \brief initialize one real-time constraint
443
 *
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
 * \param[in] delta_t_usec time step for this constraint [us]
 * \param[in] cur_t_usec current time (real-time) [us]
 * \param[in] tsim current simulation time [s]
 */
Realtime_constraint* init_realtime_constraint(int delta_t_usec, int cur_t_usec, double tsim)
{
    // variable (pointer) to return
    Realtime_constraint *constraint;

    // memory allocation
    constraint = (Realtime_constraint*) malloc(sizeof(Realtime_constraint));

    // time between 2 refresh moments (in us)
    constraint->delta_t_usec = delta_t_usec;

    // next gate for this constraint
    constraint->next_t_usec  = cur_t_usec + delta_t_usec;               // real time (in us)
    constraint->next_tsim    = tsim       + delta_t_usec * USEC_TO_SEC; // simulation time (in s)

    return constraint;
}

/*! \brief initialize the real-time structure
467
 *
468
 * \param[in,out] mbs_data Robotran main structure
469
470
471
 * \param[in] options options structure
 * \return real-time structure initialized
 */
472
Simu_realtime* init_realtime(MbsData* mbs_data, Realtime_option *options)
473
474
475
476
477
478
479
480
481
482
{
    // Real-time structure
    Simu_realtime *realtime;

    // tab with the time constraints
    double fqc_tab[NB_REAL_TIME_CONSTRAINTS];

    fqc_tab[0]  = options->fqc_screen; // constraints
    fqc_tab[1]  = options->fqc_visu;

483
    realtime = init_simu_realtime(mbs_data, options, NB_REAL_TIME_CONSTRAINTS, fqc_tab);
484
485
486
487
488

    return realtime;
}

/*! \brief initialize the structure with all the real-time constraints
489
 *
490
 * \param[in,out] mbs_data Robotran main structure
491
492
493
494
495
 * \param[in] options options structure
 * \param[in] nb_constraints number of real-time constraints
 * \param[in] fqc_tab tabular with the constraints freqeuncies
 * \return real-time structure initialized
 */
496
Simu_realtime* init_simu_realtime(MbsData* mbs_data, Realtime_option *options, int nb_constraints, double *fqc_tab)
497
498
499
{
    // -- Variables decalration -- //
    int i;
500
    int cur_t_usec;
501
    int cur_next_t_usec, min_next_t_usec;
502
503
    int init_t_sec, init_t_usec;

504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
    double cur_next_tsim, min_next_tsim;
    double tsim;

    Simu_realtime *realtime;
    Realtime_constraint **constraints;


    // -- Initialization -- //

    tsim = options->t0;

    // check the number of constraints
    if (nb_constraints < 0)
    {
        printf("Error: negative number of realtime constraints!\n");
        exit(1);
    }

    // memory allocation
    realtime = (Simu_realtime*) malloc(sizeof(Simu_realtime));

525
526
527
528
529
    realtime->ext = (Realtime_extern*) malloc(sizeof(Realtime_extern));

    realtime->ext->mbs_data = mbs_data;

    #ifdef JAVA
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
530
531
532
533
    if (options->flag_visu)
    {
        realtime->ext->java = init_realtime_java(options, mbs_data);
    }
534
535
536
537
538
539
    #else
    if (options->flag_visu)
    {
        printf("\nTo use the Java visualization (realtime->options->flag_visu activated), set the CMake flag 'FLAG_VISU' to ON !\n");
        exit(EXIT_FAILURE);
    }
540
541
542
    #endif

    #ifdef SDL
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
543
544
545
546
    if (options->flag_plot)
    {
        realtime->ext->sdl = init_realtime_sdl(init_screen_sdl(options));
    }
547
548
549
550
551
552
    #else
    if (options->flag_plot)
    {
        printf("\nTo use the real-time features (realtime->options->flag_plot activated), set the CMake flag 'FLAG_PLOT' to ON !\n");
        exit(EXIT_FAILURE);
    }
553
554
555
556
557
558
559
    #endif

    // absolute time before starting loop
    time_get(&init_t_sec, &init_t_usec);

    cur_t_usec = t_usec(init_t_sec, init_t_usec);

560
561
562
563
564
565
566
567
    // not the default time constraint
    if (nb_constraints > 0)
    {
        // flag
        realtime->no_additional_constraint = 0;

        // memory allocation: general structure
        constraints = (Realtime_constraint**) malloc(nb_constraints*sizeof(Realtime_constraint*));
568
569

        // memory allocation: each constraint (one by one)
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
        for (i=0; i<nb_constraints; i++)
        {
            #ifdef WIN32
            constraints[i] = init_realtime_constraint((int) (1e6 / fqc_tab[i]), cur_t_usec, tsim);
            #else
            constraints[i] = init_realtime_constraint((int) round(1e6 / fqc_tab[i]), cur_t_usec, tsim);
            #endif
        }
    }
    // default time constraint
    else
    {
        // flag
        realtime->no_additional_constraint = 1;

        // one default constraint
        nb_constraints = 1;

        // memory allocation: general structure
        constraints = (Realtime_constraint**) malloc(1*sizeof(Realtime_constraint*));

        // memory allocation: default constraint
        constraints[0] = init_realtime_constraint(DEFAULT_DELTA_T_USEC, cur_t_usec, tsim);
    }

    min_next_tsim   = 0.0;
    min_next_t_usec = 0;

    // get the first constraint (the most restrictive)
    for (i=0; i<nb_constraints; i++)
    {
        cur_next_tsim   = constraints[i]->next_tsim;
        cur_next_t_usec = constraints[i]->next_t_usec;

        if (!i)
        {
            min_next_tsim   = cur_next_tsim;
            min_next_t_usec = cur_next_t_usec;
        }
        else
        {
            if(cur_next_tsim < min_next_tsim)
            {
                min_next_tsim = cur_next_tsim;
            }
            if (cur_next_t_usec < min_next_t_usec)
            {
                min_next_t_usec = cur_next_t_usec;
            }
        }
    }

    realtime->next_tsim_gate   = min_next_tsim;
    realtime->next_t_usec_gate = min_next_t_usec;

    // number of constraints
    realtime->nb_constraints  = nb_constraints;

    // flags
    realtime->simu_quit       = 0;           // quit the simulation
    realtime->simu_break      = options->init_break;  // break in the simulation at the beginning
631
    realtime->last_break      = options->final_break; // break in the simulation at the end
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
632
    realtime->simu_speed_flag = get_simu_speed_flag(options->init_speed_factor); // simulation speed flag
633
634
635
636
637

    // end of the simulation time (in s)
    realtime->last_tsim = tsim;

    // simulation speed
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
638
639
    realtime->last_real_simu_speed_factor = get_simu_speed_factor(realtime->simu_speed_flag);
    realtime->real_simu_speed_factor      = realtime->last_real_simu_speed_factor;
640
641
642
643
644
645

    // constraints structure
    realtime->constraints = constraints;

    realtime->buffer_size = options->buffer_size;

646
647
    realtime->first_break = options->init_break;

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
648
649
650
    realtime->flag_plot = options->flag_plot;
    realtime->flag_visu = options->flag_visu;

651
652
    realtime->init_t_sec  = init_t_sec;
    realtime->init_t_usec = init_t_usec;
653

654
655
    realtime->speed_last_t_usec = t_usec(init_t_sec, init_t_usec);
    realtime->speed_last_tsim   = options->t0;
656

657
658
659
660
    realtime->tsim = options->t0;

    realtime->t0 = options->t0;
    realtime->tf = options->tf;
661
662
663
664
665
666

    // return the real-time structure
    return realtime;
}

/*! \brief free one real-time constraint
667
 *
668
669
670
671
672
673
674
675
 * \param[out] constraint constraint to release memory
 */
void free_realtime_constraint(Realtime_constraint* constraint)
{
    free(constraint);
}

/*! \brief free the structure of all the real-time constraints
676
 *
677
678
679
680
681
682
683
 * \param[out] realtime real-time structure to release memory
 */
void free_simu_realtime(Simu_realtime *realtime)
{
    int i;

    #ifdef SDL
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
684
685
686
687
    if (realtime->flag_plot)
    {
        free_realtime_sdl(realtime->ext->sdl);
    }
688
689
690
    #endif

    #ifdef JAVA
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
691
692
693
694
    if (realtime->flag_visu)
    {
        free_realtime_java(realtime->ext->java);
    }
695
696
697
698
699
700
701
702
703
704
    #endif

    free(realtime->ext);

    for (i=0; i<realtime->nb_constraints; i++)
    {
        free_realtime_constraint(realtime->constraints[i]);
    }

    free(realtime->constraints);
705

706
707
708
709
710
711
    free_Realtime_option(realtime->options);

    free(realtime);
}

/*! \brief update one real-time constraint
712
 *
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
 * \param[in] constraint real-time constraint
 * \param[in] simu_speed_flag index used to compute the speed factor
 */
void update_realtime_constraint(Realtime_constraint *constraint, int simu_speed_flag)
{
    int delta_t_usec;
    double simu_speed_factor;

    // simulation speed
    simu_speed_factor = get_simu_speed_factor(simu_speed_flag);

    // next gates
    delta_t_usec = constraint->delta_t_usec;

    constraint->next_t_usec += delta_t_usec;
    constraint->next_tsim   += simu_speed_factor * delta_t_usec * 1.0e-6;
}

/*! \brief update the structure of all the constraints
732
 *
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
 * \param[in,out] realtime real-time structure
 */
void update_simu_realtime(Simu_realtime *realtime)
{
    int i;
    int cur_next_t_usec, min_next_t_usec;
    double cur_next_tsim, min_next_tsim;

    min_next_t_usec = 0;
    min_next_tsim   = 0.0;

    // loop on all the constraints
    for (i=0; i<realtime->nb_constraints; i++)
    {
        cur_next_t_usec = realtime->constraints[i]->next_t_usec;
        cur_next_tsim   = realtime->constraints[i]->next_tsim;

        // first element is the minimum
        if (!i)
        {
            min_next_t_usec = cur_next_t_usec;
            min_next_tsim   = cur_next_tsim;
        }
        else
        {
            if(cur_next_t_usec < min_next_t_usec)
            {
                min_next_t_usec = cur_next_t_usec;
            }
            if(cur_next_tsim < min_next_tsim)
            {
                min_next_tsim = cur_next_tsim;
            }
        }
    }

    realtime->next_t_usec_gate = min_next_t_usec;
    realtime->next_tsim_gate   = min_next_tsim;
}

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
773
/*! \brief get simulation factor depending on the 'simu_speed_flag' flag
774
 *
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
 * \param[in] simu_speed_flag index used to compute the speed factor
 * \return speed factor
 */
double get_simu_speed_factor(int simu_speed_flag)
{
    if (simu_speed_flag < 0)
    {
        return pow_int(2.0,simu_speed_flag);
    }
    else if (simu_speed_flag > 0)
    {
        return simu_speed_flag + 1.0;
    }
    else
    {
        return 1.0;
    }
}

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
794
/*! \brief get the 'simu_speed_flag' flag depending on simulation factor
795
 *
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
 * \param[in] simu_speed_factor speed factor
 * \return index used to compute the speed factor
 */
int get_simu_speed_flag(double simu_speed_factor)
{
    double float_flag;

    // safety
    if (simu_speed_factor <= 0.0)
    {
        return 0;
    }

    if (simu_speed_factor >= 1.0)
    {
        float_flag = simu_speed_factor - 1.0;
    }
    else
    {
        float_flag = -(log(1.0/simu_speed_factor) / log(2.0));
    }

    // round to integer
    return ((float_flag - floor(float_flag) > 0.5) ? ceil(float_flag) : floor(float_flag));
}

822
/*! \brief update the real-time constraints after a break
823
  *
824
825
  * \param[in,out] realtime real-time structure
  * \param[in] delta_break_u_sec delta of time for next gate [us]
826
  */
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
void update_realtime_constraints_break(Simu_realtime *realtime, int delta_break_u_sec)
{
    int i;

    Realtime_constraint **constraints;

    constraints = realtime->constraints;

    // loop on all the constraints
    for (i=0; i<realtime->nb_constraints; i++)
    {
        constraints[i]->next_t_usec += delta_break_u_sec;
    }

    update_simu_realtime(realtime);
}

#endif