diff --git a/conf/settings/settings_booz2.xml b/conf/settings/settings_booz2.xml index ffbf7c20fa..e72437169f 100644 --- a/conf/settings/settings_booz2.xml +++ b/conf/settings/settings_booz2.xml @@ -28,7 +28,7 @@ - + diff --git a/conf/simulator/jsbsim/aircraft/BOOZ2_A1.xml b/conf/simulator/jsbsim/aircraft/BOOZ2_A1.xml index 4cc07373c8..49839a6054 100644 --- a/conf/simulator/jsbsim/aircraft/BOOZ2_A1.xml +++ b/conf/simulator/jsbsim/aircraft/BOOZ2_A1.xml @@ -236,7 +236,7 @@ 0 - -1 + 1 0 0 @@ -255,7 +255,7 @@ 0 - 1 + -1 0 0 @@ -276,7 +276,7 @@ 0 - -1 + 1 0 0 @@ -295,7 +295,7 @@ 0 - 1 + -1 0 0 @@ -317,7 +317,7 @@ 0 - -1 + 1 0 @@ -336,7 +336,7 @@ 0 - 1 + -1 0 @@ -357,7 +357,7 @@ 0 - -1 + 1 0 @@ -376,7 +376,7 @@ 0 - 1 + -1 0 diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h index 2a542fc83d..d15b3c59d5 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h @@ -33,7 +33,7 @@ #define RC_UPDATE_FREQ 40. #define ROLL_COEF (-BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ) #define PITCH_COEF ( BOOZ_STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ) -#define YAW_COEF (- BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) +#define YAW_COEF (-BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) #define YAW_DEADBAND_EXCEEDED() \ (radio_control.values[RADIO_CONTROL_YAW] > BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R || \ @@ -54,8 +54,8 @@ } \ \ struct FloatRMat sp_rmat; \ - /* FLOAT_RMAT_OF_EULERS_312(sp_rmat, _sp); */ \ - FLOAT_RMAT_OF_EULERS_321(sp_rmat, _sp); \ + FLOAT_RMAT_OF_EULERS_312(sp_rmat, _sp); \ + /* FLOAT_RMAT_OF_EULERS_321(sp_rmat, _sp);*/ \ FLOAT_QUAT_OF_RMAT(booz_stab_att_sp_quat, sp_rmat); \ /*FLOAT_EULERS_OF_QUAT(sp_euler321, sp_quat);*/ \ } diff --git a/sw/simulator/nps_autopilot_booz.c b/sw/simulator/nps_autopilot_booz.c index 44a36d3fea..6a7dba2f0b 100644 --- a/sw/simulator/nps_autopilot_booz.c +++ b/sw/simulator/nps_autopilot_booz.c @@ -56,9 +56,9 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) { booz2_main_periodic(); /* 25 */ - if (time < 5) { - double hover = 0.25; - //double hover = 0.2493; + if (time < 8) { + // double hover = 0.25; + double hover = 0.2493; // double hover = 0.23; // double hover = 0.; // if (time > 20) hover = 0.25; @@ -66,7 +66,6 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) { double pitch = 0.000; double roll = 0.0000; - autopilot.commands[SERVO_FRONT] = hover + yaw + pitch; autopilot.commands[SERVO_BACK] = hover + yaw - pitch; autopilot.commands[SERVO_RIGHT] = hover - yaw - roll ; diff --git a/sw/simulator/nps_radio_control.c b/sw/simulator/nps_radio_control.c index 4d5fc82716..5170b5ee61 100644 --- a/sw/simulator/nps_radio_control.c +++ b/sw/simulator/nps_radio_control.c @@ -8,36 +8,65 @@ void nps_radio_control_init(void) { nps_radio_control.next_update = 0.; } +static void radio_control_script_step_roll(double time); +static void radio_control_script_ff(double time); + #define MODE_SWITCH_MANUAL -1.0 #define MODE_SWITCH_AUTO1 0.0 #define MODE_SWITCH_AUTO2 1.0 -void nps_radio_control_run_script(double time) { +#define RADIO_CONTROL_TAKEOFF_TIME 8 + +void radio_control_script_takeoff(double time) { + nps_radio_control.roll = 0.; nps_radio_control.pitch = 0.; + nps_radio_control.yaw = 0.; + nps_radio_control.throttle = 0.; + nps_radio_control.mode = MODE_SWITCH_MANUAL; /* starts motors */ - if (time < 1.) { + if (time < 1.) nps_radio_control.yaw = 1.; - nps_radio_control.throttle = 0.; - nps_radio_control.mode = MODE_SWITCH_MANUAL; - } - else if (time < 3.5) { + else nps_radio_control.yaw = 0.; - nps_radio_control.throttle = 0.; - nps_radio_control.mode = MODE_SWITCH_MANUAL; + +} + +void radio_control_script_step_roll(double time) { + nps_radio_control.throttle = 0.99; + nps_radio_control.mode = MODE_SWITCH_AUTO2; + + if (((int32_t)rint((time*0.5)))%2) { + nps_radio_control.roll = 0.2; + nps_radio_control.yaw = 0.5; + } + else + nps_radio_control.roll = RadOfDeg(10); +} + +void radio_control_script_ff(double time __attribute__ ((unused))) { + nps_radio_control.throttle = 0.99; + nps_radio_control.mode = MODE_SWITCH_AUTO2; + if (time < RADIO_CONTROL_TAKEOFF_TIME+3) + nps_radio_control.pitch = -1.; + else if (time < RADIO_CONTROL_TAKEOFF_TIME+6) { + // nps_radio_control.roll = 0.5; + nps_radio_control.pitch = -1.; } else { - nps_radio_control.yaw = 0.; - nps_radio_control.throttle = 0.99; - nps_radio_control.mode = MODE_SWITCH_AUTO2; - // nps_radio_control.throttle = 0.265; - // nps_radio_control.mode = MODE_SWITCH_MANUAL; + nps_radio_control.pitch = 0.; + nps_radio_control.roll = 0.; + } +} - if (((int32_t)rint((time*0.5)))%2) - nps_radio_control.roll = 0.15; - else - nps_radio_control.roll = -0.15; + +void nps_radio_control_run_script(double time) { + if (time < RADIO_CONTROL_TAKEOFF_TIME) + radio_control_script_takeoff(time); + else { + // radio_control_script_step_roll(time); + radio_control_script_ff(time); } }