diff --git a/conf/airframes/booz2_a1.xml b/conf/airframes/booz2_a1.xml index df9f1c163c..85685af62b 100644 --- a/conf/airframes/booz2_a1.xml +++ b/conf/airframes/booz2_a1.xml @@ -76,13 +76,13 @@ - - - + + + @@ -161,7 +161,7 @@ - + @@ -179,7 +179,7 @@
- +
diff --git a/conf/airframes/booz2_a2.xml b/conf/airframes/booz2_a2.xml index 67a0522b93..0dbb12ce15 100644 --- a/conf/airframes/booz2_a2.xml +++ b/conf/airframes/booz2_a2.xml @@ -52,9 +52,9 @@ - - - + + + diff --git a/conf/airframes/booz2_a3.xml b/conf/airframes/booz2_a3.xml index 2cc1355d10..808e91b756 100644 --- a/conf/airframes/booz2_a3.xml +++ b/conf/airframes/booz2_a3.xml @@ -52,9 +52,9 @@ - - - + + + diff --git a/conf/airframes/booz2_a4.xml b/conf/airframes/booz2_a4.xml index 12c151d09a..12b7369806 100644 --- a/conf/airframes/booz2_a4.xml +++ b/conf/airframes/booz2_a4.xml @@ -54,9 +54,9 @@ - - - + + + diff --git a/conf/airframes/booz2_a5.xml b/conf/airframes/booz2_a5.xml index 0a700e2fe4..f9557e13f3 100644 --- a/conf/airframes/booz2_a5.xml +++ b/conf/airframes/booz2_a5.xml @@ -75,9 +75,9 @@ - - - + + + diff --git a/conf/autopilot/subsystems/booz_ahrs_mlkf.makefile b/conf/autopilot/subsystems/booz_ahrs_mlkf.makefile new file mode 100644 index 0000000000..c18f7b3327 --- /dev/null +++ b/conf/autopilot/subsystems/booz_ahrs_mlkf.makefile @@ -0,0 +1,14 @@ +# +# +# + +ap.CFLAGS += -DAHRS_ALIGNER_LED=3 +ap.srcs += $(SRC_BOOZ)/booz_ahrs.c +ap.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_aligner.c +ap.srcs += $(SRC_BOOZ_PRIV)/ahrs/booz_ahrs_mlkf.c + +sim.CFLAGS += -I$(SRC_BOOZ_PRIV) +sim.CFLAGS += -DAHRS_ALIGNER_LED=3 +sim.srcs += $(SRC_BOOZ)/booz_ahrs.c +sim.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_aligner.c +sim.srcs += $(SRC_BOOZ_PRIV)/ahrs/booz_ahrs_mlkf.c diff --git a/conf/simulator/nps/nps_sensors_params_booz2_a1.h b/conf/simulator/nps/nps_sensors_params_booz2_a1.h index 36db559a76..5bfb3e2da4 100644 --- a/conf/simulator/nps/nps_sensors_params_booz2_a1.h +++ b/conf/simulator/nps/nps_sensors_params_booz2_a1.h @@ -27,10 +27,10 @@ #include "airframe.h" -#if 0 -#define NPS_BODY_TO_IMU_PHI RadOfDeg(4.) -#define NPS_BODY_TO_IMU_THETA RadOfDeg(3.) -#define NPS_BODY_TO_IMU_PSI RadOfDeg(0.) +#if 1 +#define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI +#define NPS_BODY_TO_IMU_THETA IMU_BODY_TO_IMU_THETA +#define NPS_BODY_TO_IMU_PSI IMU_BODY_TO_IMU_PSI #else #define NPS_BODY_TO_IMU_PHI RadOfDeg(0.) #define NPS_BODY_TO_IMU_THETA RadOfDeg(0.) diff --git a/sw/airborne/booz/ahrs/booz2_filter_attitude_cmpl_euler.c b/sw/airborne/booz/ahrs/booz2_filter_attitude_cmpl_euler.c index f1afa96881..ca09ca90a9 100644 --- a/sw/airborne/booz/ahrs/booz2_filter_attitude_cmpl_euler.c +++ b/sw/airborne/booz/ahrs/booz2_filter_attitude_cmpl_euler.c @@ -196,9 +196,13 @@ void booz_ahrs_propagate(void) { } -void booz_ahrs_update(void) { +void booz_ahrs_update_accel(void) { } +void booz_ahrs_update_mag(void) { + + +} diff --git a/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c b/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c index 393fefbee2..961ebcd0f5 100644 --- a/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c +++ b/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c @@ -35,6 +35,8 @@ #define BAFL_DEBUG +static void booz_ahrs_do_update_accel(void); +static void booz_ahrs_do_update_mag(void); /* our estimated attitude (ltp <-> imu) */ @@ -403,8 +405,11 @@ void booz_ahrs_propagate(void) { #endif } - void booz_ahrs_update_accel(void) { + RunOnceEvery(50, booz_ahrs_do_update_accel()); +} + +static void booz_ahrs_do_update_accel(void) { int i, j, k; #ifdef BAFL_DEBUG2 @@ -632,6 +637,10 @@ void booz_ahrs_update_accel(void) { void booz_ahrs_update_mag(void) { + RunOnceEvery(10, booz_ahrs_do_update_mag()); +} + +static void booz_ahrs_update_mag(void) { int i, j, k; #ifdef BAFL_DEBUG2 diff --git a/sw/airborne/booz/booz_ahrs.h b/sw/airborne/booz/booz_ahrs.h index d309781aba..c9fd54b9ff 100644 --- a/sw/airborne/booz/booz_ahrs.h +++ b/sw/airborne/booz/booz_ahrs.h @@ -33,43 +33,53 @@ #define BOOZ_AHRS_RUNNING 1 struct BoozAhrs { - struct Int32Eulers ltp_to_body_euler; - struct Int32Quat ltp_to_body_quat; - struct Int32RMat ltp_to_body_rmat; + + struct Int32Quat ltp_to_imu_quat; struct Int32Eulers ltp_to_imu_euler; - struct Int32Quat ltp_to_imu_quat; - struct Int32RMat ltp_to_imu_rmat; - struct Int32Rates imu_rate; - struct Int32Rates body_rate; + struct Int32RMat ltp_to_imu_rmat; + struct Int32Rates imu_rate; + + struct Int32Quat ltp_to_body_quat; + struct Int32Eulers ltp_to_body_euler; + struct Int32RMat ltp_to_body_rmat; + struct Int32Rates body_rate; + uint8_t status; }; struct BoozAhrsFloat { struct FloatQuat ltp_to_imu_quat; struct FloatEulers ltp_to_imu_euler; + struct FloatRMat ltp_to_imu_rmat; struct FloatRates imu_rate; struct FloatQuat ltp_to_body_quat; struct FloatEulers ltp_to_body_euler; + struct FloatRMat ltp_to_body_rmat; struct FloatRates body_rate; + uint8_t status; }; extern struct BoozAhrs booz_ahrs; extern struct BoozAhrsFloat booz_ahrs_float; -#define BOOZ_AHRS_FLOAT_OF_INT32() { \ - QUAT_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_quat, booz_ahrs.ltp_to_body_quat); \ - RATES_FLOAT_OF_BFP(booz_ahrs_float.body_rate, booz_ahrs.body_rate); \ - booz_ahrs_float.ltp_to_body_euler.phi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.phi); \ - booz_ahrs_float.ltp_to_body_euler.theta = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.theta); \ - booz_ahrs_float.ltp_to_body_euler.psi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.psi); \ +#define BOOZ_AHRS_FLOAT_OF_INT32() { \ + QUAT_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_quat, booz_ahrs.ltp_to_body_quat); \ + EULERS_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_euler, booz_ahrs.ltp_to_body_euler); \ + RATES_FLOAT_OF_BFP(booz_ahrs_float.body_rate, booz_ahrs.body_rate); \ + } + +#define BOOZ_AHRS_INT_OF_FLOAT() { \ + QUAT_BFP_OF_REAL(booz_ahrs.ltp_to_body_quat, booz_ahrs_float.ltp_to_body_quat); \ + EULERS_BFP_OF_REAL(booz_ahrs.ltp_to_body_euler, booz_ahrs_float.ltp_to_body_euler); \ + RMAT_BFP_OF_REAL(booz_ahrs.ltp_to_body_rmat, booz_ahrs_float.ltp_to_body_rmat); \ + RATES_BFP_OF_REAL(booz_ahrs.body_rate, booz_ahrs_float.body_rate); \ } extern void booz_ahrs_init(void); extern void booz_ahrs_align(void); extern void booz_ahrs_propagate(void); -extern void booz_ahrs_update(void); extern void booz_ahrs_update_accel(void); extern void booz_ahrs_update_mag(void); diff --git a/sw/airborne/booz/booz_imu.c b/sw/airborne/booz/booz_imu.c index 3cc7eb6412..2de66a4e06 100644 --- a/sw/airborne/booz/booz_imu.c +++ b/sw/airborne/booz/booz_imu.c @@ -38,7 +38,10 @@ void booz_imu_init(void) { Compute quaternion and rotation matrix for conversions between body and imu frame */ - struct Int32Eulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; + struct Int32Eulers body_to_imu_eulers = + { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI), + ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), + ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; INT32_QUAT_OF_EULERS(booz_imu.body_to_imu_quat, body_to_imu_eulers); INT32_QUAT_NORMALISE(booz_imu.body_to_imu_quat); INT32_RMAT_OF_EULERS(booz_imu.body_to_imu_rmat, body_to_imu_eulers); diff --git a/sw/airborne/booz/fms/booz_fms_test_signal.c b/sw/airborne/booz/fms/booz_fms_test_signal.c index e2ef096272..9846ed62e6 100644 --- a/sw/airborne/booz/fms/booz_fms_test_signal.c +++ b/sw/airborne/booz/fms/booz_fms_test_signal.c @@ -63,6 +63,9 @@ void booz_fms_impl_periodic(void) { } } break; + case STEP_PITCH: + case STEP_VERT: + break; #if 0 case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: { if (booz2_guidance_v_mode < BOOZ2_GUIDANCE_V_MODE_HOVER)