You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
237 lines
5.4 KiB
237 lines
5.4 KiB
#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;
|
|
}
|
|
|