|
|
- #include <unistd.h> // For sleep()
- #include <pthread.h>
- #include "types.h"
- #include "assemblage_includes.h"
- #include "assemblage.h"
-
- // This should be set to 1 to run in "real-time" in the sense
- // that the simulation time is close to the real world time
- #define RUN_WITH_REAL_TIME 0
-
- // Task set
- struct nonencoded_task_params* tasks;
-
- // I/O
- output_t outs;
- uint64_t step_simu;
- uint64_t max_step_simu;
-
- // Barriers for thread synchro
- pthread_barrier_t cycle_start_b;
- pthread_barrier_t engine_elevator_b;
- pthread_barrier_t filter_b;
- pthread_barrier_t control_b;
- pthread_barrier_t output_b;
-
- // Output variables
- extern double aircraft_dynamics495_Va_Va_filter_100449_Va[2];
- extern double aircraft_dynamics495_az_az_filter_100458_az[2];
- extern double aircraft_dynamics495_Vz_Vz_filter_100452_Vz[2];
- extern double aircraft_dynamics495_q_q_filter_100455_q[2];
- extern double aircraft_dynamics495_h_h_filter_100446_h[2];
- extern double Va_control_50474_delta_th_c_delta_th_c;
- extern double Vz_control_50483_delta_e_c_delta_e_c;
-
- void copy_output_vars(output_t* v, uint64_t step){
- v->sig_outputs.Va = aircraft_dynamics495_Va_Va_filter_100449_Va[step%2];
- v->sig_outputs.Vz = aircraft_dynamics495_Vz_Vz_filter_100452_Vz[step%2];
- v->sig_outputs.q = aircraft_dynamics495_q_q_filter_100455_q[step%2];
- v->sig_outputs.az = aircraft_dynamics495_az_az_filter_100458_az[step%2];
- v->sig_outputs.h = aircraft_dynamics495_h_h_filter_100446_h[step%2];
- v->sig_delta_th_c = Va_control_50474_delta_th_c_delta_th_c;
- v->sig_delta_e_c = Vz_control_50483_delta_e_c_delta_e_c;
- }
-
- void rosace_init() {
- // Init barriers
- pthread_barrier_init(&cycle_start_b, NULL, 5);
- pthread_barrier_init(&engine_elevator_b, NULL, 2);
- pthread_barrier_init(&filter_b, NULL, 5);
- pthread_barrier_init(&control_b, NULL, 2);
- pthread_barrier_init(&output_b, NULL, 2);
-
- // Initial values
- outs.sig_outputs.Va = 0;
- outs.sig_outputs.Vz = 0;
- outs.sig_outputs.q = 0;
- outs.sig_outputs.az = 0;
- outs.sig_outputs.h = 0;
- outs.t_simu = 0;
- step_simu = 0;
-
- // Get the task set (required for CALL() macro)
- int tmp;
- get_task_set(&tmp, &tasks);
- }
-
-
-
- #define CALL(val) tasks[(val)].ne_t_body(NULL)
-
- void* thread1(void* arg) {
- uint64_t mystep_simu = step_simu;
- while(step_simu<max_step_simu) {
- pthread_barrier_wait(&cycle_start_b);
-
- // --- 200 Hz ---
- CALL(ENGINE);
- pthread_barrier_wait(&engine_elevator_b);
- // --- End 200 Hz ---
-
- // --- 100 Hz ---
- if(mystep_simu%2 == 0) {
- pthread_barrier_wait(&filter_b);
- CALL(VZ_FILTER);
- }
- // --- End 100 Hz ---
-
- // --- 10 Hz ---
- if(mystep_simu%20 == 0)
- CALL(VA_C0);
- // --- End 10 Hz ---
-
- // --- 50 Hz ---
- if(mystep_simu%4 == 0) {
- pthread_barrier_wait(&control_b);
- CALL(VA_CONTROL);
- }
- if(mystep_simu%4 == 3) {
- pthread_barrier_wait(&output_b);
- CALL(DELTA_TH_C0);
- }
- // --- End 50 Hz ---
-
- step_simu = step_simu + 1;
- outs.t_simu += 5;
-
- // Print output
- copy_output_vars(&outs, step_simu);
- if (step_simu%10)
- ROSACE_write_outputs(&outs);
-
- if(RUN_WITH_REAL_TIME)
- usleep(5000); // "Real-time" execution
-
- mystep_simu++;
- }
- return NULL;
- }
-
- void* thread2(void* arg) {
- uint64_t mystep_simu = step_simu;
- while(step_simu<max_step_simu) {
- pthread_barrier_wait(&cycle_start_b);
-
- // --- 200 Hz ---
- CALL(ELEVATOR);
- pthread_barrier_wait(&engine_elevator_b);
-
- CALL(AIRCRAFT_DYN);
- // --- End 200 Hz ---
-
- // --- 100 Hz ---
- if(mystep_simu%2 == 0) {
- pthread_barrier_wait(&filter_b);
- CALL(H_FILTER);
- }
- // --- End 100 Hz ---
-
- // --- 10 Hz ---
- if(mystep_simu%20 == 0)
- CALL(H_C0);
- // --- End 10 Hz ---
-
- // --- 50 Hz ---
- if(mystep_simu%4 == 0) {
- CALL(ALTI_HOLD);
- pthread_barrier_wait(&control_b);
- CALL(VZ_CONTROL);
- }
- if(mystep_simu%4 == 3) {
- pthread_barrier_wait(&output_b);
- CALL(DELTA_E_C0);
- }
- // --- End 50 Hz ---
-
- mystep_simu++;
- }
- return NULL;
- }
-
- void* thread3(void* arg) {
- uint64_t mystep_simu = step_simu;
- while(step_simu<max_step_simu) {
- pthread_barrier_wait(&cycle_start_b);
- // --- 100 Hz ---
- if(mystep_simu%2 == 0) {
- pthread_barrier_wait(&filter_b);
- CALL(Q_FILTER);
- }
- // --- End 100 Hz ---
- mystep_simu++;
- }
- return NULL;
- }
-
- void* thread4(void* arg) {
- uint64_t mystep_simu = step_simu;
- while(step_simu<max_step_simu) {
- pthread_barrier_wait(&cycle_start_b);
- // --- 100 Hz ---
- if(mystep_simu%2 == 0) {
- pthread_barrier_wait(&filter_b);
- CALL(VA_FILTER);
- }
- // --- End 100 Hz ---
- mystep_simu++;
- }
- return NULL;
- }
-
- void* thread5(void* arg) {
- uint64_t mystep_simu = step_simu;
- while(step_simu<max_step_simu) {
- pthread_barrier_wait(&cycle_start_b);
- // --- 100 Hz ---
- if(mystep_simu%2 == 0) {
- pthread_barrier_wait(&filter_b);
- CALL(AZ_FILTER);
- }
- // --- End 100 Hz ---
- mystep_simu++;
- }
- return NULL;
- }
-
-
- int run_rosace(uint64_t nbstep){
- int i;
- // Variables for thread management
- void* fcts[] = {&thread1, &thread2, &thread3, &thread4, &thread5};
- pthread_t threads[5];
-
- rosace_init();
- max_step_simu = nbstep;
- // Set first command
- ROSACE_update_altitude_command(11000.0);
-
- // Create the 5 threads
- for(i=0; i<5; i++)
- pthread_create( &(threads[i]), NULL, fcts[i], NULL);
-
- // SCENARIO
- /* sleep(20);
- ROSACE_update_altitude_command(10500.0);
- sleep(20);
- ROSACE_update_altitude_command(11500.0);
- sleep(20);
- ROSACE_update_altitude_command(10000.0);
- sleep(20);
- ROSACE_update_altitude_command(9000.0);
- */
- // Exit
- for(i=0; i<5; i++)
- pthread_join(threads[i], NULL);
- return 0;
- }
-
|