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);
}
}