diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml
index bdcfc2c4ea..e3232c32fb 100644
--- a/conf/telemetry/telemetry_booz2.xml
+++ b/conf/telemetry/telemetry_booz2.xml
@@ -64,7 +64,7 @@
-
+
diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h
index f30c01f89e..38ca8765fe 100644
--- a/sw/airborne/math/pprz_algebra.h
+++ b/sw/airborne/math/pprz_algebra.h
@@ -476,9 +476,9 @@
(_qi).qz = QUAT1_BFP_OF_REAL((_qf).qz); \
}
-#define RATES_FLOAT_OF_BFP(_ef, _ei) { \
+#define RATES_FLOAT_OF_BFP(_ef, _ei) { \
(_ef).phi = RATE_FLOAT_OF_BFP((_ei).phi); \
- (_ef).theta = RATE_FLOAT_OF_BFP((_ei).theta); \
+ (_ef).theta = RATE_FLOAT_OF_BFP((_ei).theta); \
(_ef).psi = RATE_FLOAT_OF_BFP((_ei).psi); \
}
diff --git a/sw/simulator/nps_autopilot_booz.c b/sw/simulator/nps_autopilot_booz.c
index 78d9fb6a16..580efcf2e6 100644
--- a/sw/simulator/nps_autopilot_booz.c
+++ b/sw/simulator/nps_autopilot_booz.c
@@ -92,6 +92,9 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
#include "math/pprz_algebra_int.h"
#include "booz_ahrs.h"
static void sim_overwrite_ahrs(void) {
+
+ // printf("%f\n", fdm.ltpprz_to_body_eulers.phi);
+
booz_ahrs.ltp_to_body_euler.phi = ANGLE_BFP_OF_REAL(fdm.ltpprz_to_body_eulers.phi);
booz_ahrs.ltp_to_body_euler.theta = ANGLE_BFP_OF_REAL(fdm.ltpprz_to_body_eulers.theta);
booz_ahrs.ltp_to_body_euler.psi = ANGLE_BFP_OF_REAL(fdm.ltpprz_to_body_eulers.psi);
diff --git a/sw/simulator/nps_fdm_jsbsim.c b/sw/simulator/nps_fdm_jsbsim.c
index 2c90e7f1f8..84fb864f9e 100644
--- a/sw/simulator/nps_fdm_jsbsim.c
+++ b/sw/simulator/nps_fdm_jsbsim.c
@@ -95,9 +95,13 @@ static void fetch_state(void) {
jsbsimvec_to_vec(&fdm.body_ecef_accel,&propagate->GetUVWdot());
//jsbsimvec_to_vec(&noninertial_accel,&propagate->GetUVWdot());
+ /* attitude */
jsbsimquat_to_quat(&fdm.ltp_to_body_quat,&VState->vQtrn);
+ /* convert to eulers */
+ DOUBLE_EULERS_OF_QUAT(fdm.ltp_to_body_eulers, fdm.ltp_to_body_quat);
jsbsimvec_to_rate(&fdm.body_ecef_rotvel,&VState->vPQR);
jsbsimvec_to_rate(&fdm.body_ecef_rotaccel,&propagate->GetPQRdot());
+ /* JSBSIM seems to call GetPQRdot body_ecef_rotaccel */
// rate_to_vec(&dummy_vector,&fdm.body_ecef_rotvel);
// DOUBLE_VECT3_CROSS_PRODUCT(fdm.body_ecef_accel,dummy_vector,fdm.body_ecef_vel);
// DOUBLE_VECT3_SUM(fdm.body_ecef_accel,fdm.body_ecef_accel,noninertial_accel)
@@ -107,10 +111,14 @@ static void fetch_state(void) {
struct NedCoor_f ned;
ned_of_ecef_point_f(&ned, <pdef, &ecefpos_f);
VECT3_COPY(fdm.ltpprz_pos,ned);
- DOUBLE_EULERS_OF_QUAT(fdm.ltp_to_body_eulers, fdm.ltp_to_body_quat);
//jsbsimloc_to_lla(&fdm.lla_pos, &VState->vLocation);
test123(&fdm.lla_pos, propagate);
+
+ /* the "false" pprz lpt */
+ /* FIXME: use jsbsim lpt for now */
+ EULERS_COPY(fdm.ltpprz_to_body_eulers, fdm.ltp_to_body_eulers);
+ QUAT_COPY(fdm.ltpprz_to_body_quat, fdm.ltp_to_body_quat);
}
diff --git a/sw/simulator/nps_radio_control.c b/sw/simulator/nps_radio_control.c
index ca97cf91a7..4d5fc82716 100644
--- a/sw/simulator/nps_radio_control.c
+++ b/sw/simulator/nps_radio_control.c
@@ -33,10 +33,10 @@ void nps_radio_control_run_script(double time) {
// nps_radio_control.throttle = 0.265;
// nps_radio_control.mode = MODE_SWITCH_MANUAL;
- if (((int32_t)rint((time*1.)))%2)
- nps_radio_control.roll = 0.1;
+ if (((int32_t)rint((time*0.5)))%2)
+ nps_radio_control.roll = 0.15;
else
- nps_radio_control.roll = -0.1;
+ nps_radio_control.roll = -0.15;
}
}