diff --git a/sw/simulator/sim_ac_fw.c b/sw/simulator/sim_ac_fw.c index 34695141e5..0f6ad702cb 100644 --- a/sw/simulator/sim_ac_fw.c +++ b/sw/simulator/sim_ac_fw.c @@ -30,7 +30,40 @@ using namespace JSBSim; +/* Datalink Ivy function */ +static void on_DL_PING(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), + int argc __attribute__ ((unused)), char *argv[]){ + parse_dl_ping(argv); +} +static void on_DL_ACINFO(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), + int argc __attribute__ ((unused)), char *argv[]){ + parse_dl_acinfo(argv); +} +static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), + int argc __attribute__ ((unused)), char *argv[]){ + parse_dl_setting(argv); +} +static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), + int argc __attribute__ ((unused)), char *argv[]){ + parse_dl_block(argv); +} +static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), + int argc __attribute__ ((unused)), char *argv[]){ + parse_dl_move_wp(argv); +} + void autopilot_init(void) { + IvyBindMsg(on_DL_PING, NULL, "^(\\S*) DL_PING"); + IvyBindMsg(on_DL_ACINFO, NULL, "^(\\S*) DL_ACINFO (\\S*) (\\S*) (\\S* (\\S*) (\\S*) (\\S*)) (\\S*) (\\S*)"); + IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)"); + IvyBindMsg(on_DL_BLOCK, NULL, "^(\\S*) BLOCK (\\S*) (\\S*)"); + IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); + init_fbw(); init_ap(); } @@ -51,17 +84,17 @@ void print(FGFDMExec* FDMExec) { const char* state[] = {"sim-time-sec", "position/lat-gc-deg","position/long-gc-deg","position/h-sl-meters",/* "ic/lat-gc-deg","ic/long-gc-deg","ic/h-sl-ft", - "velocities/v-north-fps","velocities/v-east-fps","velocities/v-down-fps","velocities/vg-fps", - "attitude/roll-rad","attitude/pitch-rad","attitude/heading-true-rad", + "velocities/v-north-fps","velocities/v-east-fps","velocities/v-down-fps","velocities/vg-fps",*/ + "attitude/roll-rad","attitude/pitch-rad","attitude/heading-true-rad",/* "velocities/p-rad_sec","velocities/q-rad_sec","velocities/r-rad_sec",*/ - "forces/fbx-gear-lbs","forces/fby-gear-lbs","forces/fbz-gear-lbs"}; + "fcs/throttle-cmd-norm","fcs/aileron-cmd-norm","fcs/elevator-cmd-norm"}; int i=0; cur_node = FDMExec->GetPropertyManager()->GetNode("sim-time-sec"); cur_value = cur_node->getDoubleValue(); cout << state[i] << " " << cur_value << endl; - for (i=1; i<6+1; i++) { + for (i=1; i<9+1; i++) { if (strstr(state[i],"rad_")!=NULL) factor=RAD2DEG; if (strstr(state[i],"fps")!=NULL || strstr(state[i],"ft")!=NULL) factor=FT2M; cur_node = FDMExec->GetPropertyManager()->GetNode(state[i]); @@ -71,12 +104,36 @@ void print(FGFDMExec* FDMExec) { } } -void copy_inputs_to_jsbsim(FGFDMExec* FDMExec) { +static inline void set_value(FGFDMExec* FDMExec, string name, double value) { + FDMExec->GetPropertyManager()->GetNode(name)->setDoubleValue(value); +} +static inline double normalize_from_pprz(int command) { + double cmd_norm = (double)command / MAX_PPRZ; + BoundAbs(cmd_norm, MAX_PPRZ); + return cmd_norm; +} + +void copy_inputs_to_jsbsim(FGFDMExec* FDMExec) { + // detect launch + if (!run_model && launch && !kill_throttle) { + run_model = true; + // set initial speed + //FDMExec->GetIC()->SetVgroundFpsIC(15./FT2M); + //FDMExec->RunIC(); + } + + set_value(FDMExec, "fcs/throttle-cmd-norm", normalize_from_pprz(commands[COMMAND_THROTTLE])/3); + //set_value(FDMExec, "fcs/throttle-cmd-norm", 0.2); + set_value(FDMExec, "fcs/aileron-cmd-norm", -normalize_from_pprz(commands[COMMAND_ROLL])); + set_value(FDMExec, "fcs/elevator-cmd-norm", -normalize_from_pprz(commands[COMMAND_PITCH])); +#ifdef COMMAND_YAW + set_value(FDMExec, "fcs/rudder-cmd-norm", normalize_from_pprz(commands[COMMAND_YAW])); +#endif } -double get_value(FGFDMExec* FDMExec, string name) { +static inline double get_value(FGFDMExec* FDMExec, string name) { return FDMExec->GetPropertyManager()->GetNode(name)->getDoubleValue(); } @@ -100,7 +157,7 @@ void copy_outputs_from_jsbsim(FGFDMExec* FDMExec) { gps_period = 0.; } - print(FDMExec); + //print(FDMExec); // copy IR double roll = get_value(FDMExec, "attitude/roll-rad"); double pitch = get_value(FDMExec, "attitude/pitch-rad"); diff --git a/sw/simulator/sim_ac_jsbsim.c b/sw/simulator/sim_ac_jsbsim.c index 8427e1e204..ff749d8f82 100644 --- a/sw/simulator/sim_ac_jsbsim.c +++ b/sw/simulator/sim_ac_jsbsim.c @@ -28,13 +28,15 @@ #include #include -#include +//#include #include /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% GLOBAL DATA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ +bool run_model; + string ICName; string AircraftName; JSBSim::FGFDMExec* FDMExec; @@ -45,13 +47,12 @@ static gboolean sim_periodic(gpointer data); string ivyBus = "127.255.255.255"; static void ivy_transport_init(void); -//static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)), -// void *user_data __attribute__ ((unused)), -// int argc __attribute__ ((unused)), char *argv[]); static void sim_init(void) { - + + run_model = false; + jsbsim_init(); // init sensors ? or discribe them in jSBSim @@ -70,9 +71,9 @@ static gboolean sim_periodic(gpointer data __attribute__ ((unused))) { /* run JSBSim flight model */ bool result = true; - //if (check_crash_jsbsim(FDMExec)) { + if (run_model) { result = FDMExec->Run(); - //} + } /* check if still flying */ //result = check_crash_jsbsim(FDMExec); @@ -209,10 +210,10 @@ void jsbsim_init(void) { // Use flight plan initial conditions IC->SetLatitudeDegIC(NAV_LAT0 / 1e7); IC->SetLongitudeDegIC(NAV_LON0 / 1e7); - IC->SetAltitudeFtIC((GROUND_ALT+1) / FT2M); + IC->SetAltitudeFtIC((GROUND_ALT+100) / FT2M); IC->SetTerrainAltitudeFtIC(GROUND_ALT / FT2M); IC->SetPsiDegIC(QFU); - IC->SetVgroundFpsIC(15./FT2M); + IC->SetVgroundFpsIC(0.); if (!FDMExec->RunIC()) { cerr << "Initialization from flight plan unsuccessful" << endl; exit(-1); @@ -235,13 +236,10 @@ void jsbsim_init(void) { } bool check_crash_jsbsim(JSBSim::FGFDMExec* FDMExec) { - - JSBSim::FGPropertyManager* cur_node; - double cur_value; - - cur_node = FDMExec->GetPropertyManager()->GetNode("position/h-agl-ft"); - cur_value = cur_node->getDoubleValue(); - - if (cur_value>=0) return true; - else return false; + double agl = FDMExec->GetPropertyManager()->GetNode("position/h-agl-ft")->getDoubleValue(); + if (agl>=0) return true; + else { + cerr << "Crash detected" << endl; + return false; + } } diff --git a/sw/simulator/sim_ac_jsbsim.h b/sw/simulator/sim_ac_jsbsim.h index db3ae16cef..c4ab832534 100644 --- a/sw/simulator/sim_ac_jsbsim.h +++ b/sw/simulator/sim_ac_jsbsim.h @@ -33,6 +33,7 @@ #include "airframe.h" #include "flight_plan.h" +#include /* 60Hz <-> 17ms */ #ifndef JSBSIM_PERIOD @@ -43,6 +44,8 @@ #define RAD2DEG 57.29578 #define FT2M 0.3048 +extern bool run_model; + void autopilot_init(void); void autopilot_periodic_task(void); void autopilot_event_task(void);