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