diff --git a/conf/settings/estimation/body_to_imu.xml b/conf/settings/estimation/body_to_imu.xml new file mode 100644 index 0000000000..57e07124c9 --- /dev/null +++ b/conf/settings/estimation/body_to_imu.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c index 0e68dc22f8..99c860b6fe 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c @@ -346,7 +346,8 @@ void v_ctl_climb_loop( void ) // Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration #ifndef SITL struct Int32Vect3 accel_meas_body; - INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); + INT32_RMAT_TRANSP_VMULT(accel_meas_body, *body_to_imu_rmat, imu.accel); float vdot = ACCEL_FLOAT_OF_BFP(accel_meas_body.x) / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta); #else float vdot = 0; diff --git a/sw/airborne/modules/nav/nav_catapult.c b/sw/airborne/modules/nav/nav_catapult.c index f8a9da3b9d..a147ca2c67 100644 --- a/sw/airborne/modules/nav/nav_catapult.c +++ b/sw/airborne/modules/nav/nav_catapult.c @@ -109,7 +109,8 @@ void nav_catapult_highrate_module(void) // Five consecutive measurements > 1.5 #ifndef SITL struct Int32Vect3 accel_meas_body; - INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); + INT32_RMAT_TRANSP_VMULT(accel_meas_body, *body_to_imu_rmat, imu.accel); if (ACCEL_FLOAT_OF_BFP(accel_meas_body.x) < (nav_catapult_acceleration_threshold * 9.81)) #else if (launch != 1) diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index 3a8963cd01..2977c599cc 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -171,15 +171,11 @@ void ahrs_init(void) { ahrs_impl.ltp_vel_norm_valid = FALSE; ahrs_impl.heading_aligned = FALSE; - /* Initialises IMU alignement */ - struct FloatEulers body_to_imu_euler = - {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; - FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler); - FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); - /* Set ltp_to_imu so that body is zero */ - QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); - RMAT_COPY(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); + memcpy(&ahrs_impl.ltp_to_imu_quat, orientationGetQuat_f(&imu.body_to_imu), + sizeof(struct FloatQuat)); + memcpy(&ahrs_impl.ltp_to_imu_rmat, orientationGetRMat_f(&imu.body_to_imu), + sizeof(struct FloatRMat)); FLOAT_RATES_ZERO(ahrs_impl.imu_rate); @@ -295,7 +291,8 @@ void ahrs_update_accel(void) { /* convert centrifugal acceleration from body to imu frame */ struct FloatVect3 acc_c_imu; - FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body); + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu); + FLOAT_RMAT_VECT3_MUL(acc_c_imu, *body_to_imu_rmat, acc_c_body); /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */ VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu); @@ -556,7 +553,8 @@ void ahrs_realign_heading(float heading) { FLOAT_QUAT_COMP_NORM_SHORTEST(q, q_c, *ltp_to_body_quat); /* compute ltp to imu rotation quaternion and matrix */ - FLOAT_QUAT_COMP(ahrs_impl.ltp_to_imu_quat, q, ahrs_impl.body_to_imu_quat); + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu); + FLOAT_QUAT_COMP(ahrs_impl.ltp_to_imu_quat, q, *body_to_imu_quat); FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); /* set state */ @@ -572,7 +570,8 @@ void ahrs_realign_heading(float heading) { static inline void compute_body_orientation_and_rates(void) { /* Compute LTP to BODY quaternion */ struct FloatQuat ltp_to_body_quat; - FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu); + FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, *body_to_imu_quat); /* Set state */ #ifdef AHRS_UPDATE_FW_ESTIMATOR struct FloatEulers neutrals_to_body_eulers = { ins_roll_neutral, ins_pitch_neutral, 0. }; @@ -587,6 +586,7 @@ static inline void compute_body_orientation_and_rates(void) { /* compute body rates */ struct FloatRates body_rate; - FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate); + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu); + FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_impl.imu_rate); stateSetBodyRates_f(&body_rate); } diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h index a3b83ba6b7..cca2281eb2 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h @@ -56,14 +56,6 @@ struct AhrsFloatCmpl { bool_t heading_aligned; struct FloatVect3 mag_h; - - /* - Holds float version of IMU alignement - in order to be able to run against the fixed point - version of the IMU - */ - struct FloatQuat body_to_imu_quat; - struct FloatRMat body_to_imu_rmat; }; extern struct AhrsFloatCmpl ahrs_impl; diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index cd7383ef76..f1a8b86f03 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -124,20 +124,14 @@ static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat) void ahrs_init(void) { ahrs.status = AHRS_UNINIT; - /* - * Initialises our IMU alignement variables - * This should probably done in the IMU code instead - */ - struct FloatEulers body_to_imu_euler = - {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; - FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); - - EULERS_COPY(ahrs_impl.ltp_to_imu_euler, body_to_imu_euler); + /* Set ltp_to_imu so that body is zero */ + memcpy(&ahrs_impl.ltp_to_imu_euler, orientationGetEulers_f(&imu.body_to_imu), + sizeof(struct FloatEulers)); FLOAT_RATES_ZERO(ahrs_impl.imu_rate); /* set inital filter dcm */ - set_dcm_matrix_from_rmat(&ahrs_impl.body_to_imu_rmat); + set_dcm_matrix_from_rmat(orientationGetRMat_f(&imu.body_to_imu)); ahrs_impl.gps_speed = 0; ahrs_impl.gps_acceleration = 0; @@ -387,7 +381,7 @@ void Normalize(void) // Reset on trouble if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down! - set_dcm_matrix_from_rmat(&ahrs_impl.body_to_imu_rmat); + set_dcm_matrix_from_rmat(orientationGetRMat_f(&imu.body_to_imu)); problem = FALSE; } } @@ -534,13 +528,15 @@ void Matrix_update(void) */ static inline void set_body_orientation_and_rates(void) { + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu); + struct FloatRates body_rate; - FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate); + FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_impl.imu_rate); stateSetBodyRates_f(&body_rate); struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat; FLOAT_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler); - FLOAT_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); + FLOAT_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, *body_to_imu_rmat); // Some stupid lines of code for neutrals struct FloatEulers ltp_to_body_euler; diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h index 0f92e10ce5..7b1f9f0e50 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h @@ -32,7 +32,6 @@ struct AhrsFloatDCM { struct FloatRates rate_correction; struct FloatEulers ltp_to_imu_euler; - struct FloatRMat body_to_imu_rmat; struct FloatRates imu_rate; float gps_speed; diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c index c396e4c898..5346bb4823 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c @@ -77,17 +77,9 @@ void ahrs_init(void) { ahrs.status = AHRS_UNINIT; - /* - * Initialises our IMU alignement variables - * This should probably done in the IMU code instead - */ - struct FloatEulers body_to_imu_euler = - {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; - FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler); - FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); - /* Set ltp_to_imu so that body is zero */ - QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); + memcpy(&ahrs_impl.ltp_to_imu_quat, orientationGetQuat_f(&imu.body_to_imu), + sizeof(struct FloatQuat)); FLOAT_RATES_ZERO(ahrs_impl.imu_rate); @@ -289,16 +281,18 @@ static inline void reset_state(void) { * Compute body orientation and rates from imu orientation and rates */ static inline void set_body_state_from_quat(void) { + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu); + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu); /* Compute LTP to BODY quaternion */ struct FloatQuat ltp_to_body_quat; - FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); + FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, *body_to_imu_quat); /* Set in state interface */ stateSetNedToBodyQuat_f(<p_to_body_quat); /* compute body rates */ struct FloatRates body_rate; - FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate); + FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_impl.imu_rate); /* Set state */ stateSetBodyRates_f(&body_rate); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h index 5f811594c4..58a4e3c32d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h @@ -48,9 +48,6 @@ struct AhrsMlkf { struct FloatQuat gibbs_cor; float P[6][6]; float lp_accel; - // Holds float version of IMU alignement - struct FloatQuat body_to_imu_quat; - struct FloatRMat body_to_imu_rmat; }; extern struct AhrsMlkf ahrs_impl; diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.c b/sw/airborne/subsystems/ahrs/ahrs_gx3.c index 052018a9b2..275e1dcec6 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_gx3.c +++ b/sw/airborne/subsystems/ahrs/ahrs_gx3.c @@ -258,13 +258,14 @@ void gx3_packet_read_message(void) { struct FloatRates body_rate; imuf.gyro = ahrs_impl.gx3_rate; /* compute body rates */ - FLOAT_RMAT_TRANSP_RATEMULT(body_rate, imuf.body_to_imu_rmat, imuf.gyro); + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imuf.body_to_imu); + FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, imuf.gyro); /* Set state */ stateSetBodyRates_f(&body_rate); // Attitude struct FloatRMat ltp_to_body_rmat; - FLOAT_RMAT_COMP(ltp_to_body_rmat, ahrs_impl.gx3_rmat, imuf.body_to_imu_rmat); + FLOAT_RMAT_COMP(ltp_to_body_rmat, ahrs_impl.gx3_rmat, *body_to_imu_rmat); #ifdef AHRS_UPDATE_FW_ESTIMATOR // fixedwing struct FloatEulers ltp_to_body_eulers; FLOAT_EULERS_OF_RMAT(ltp_to_body_eulers, ltp_to_body_rmat); @@ -327,7 +328,8 @@ void gx3_packet_parse( uint8_t c ) { void ahrs_init(void) { ahrs.status = AHRS_UNINIT; /* set ltp_to_imu so that body is zero */ - QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imuf.body_to_imu_quat); + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imuf.body_to_imu); + QUAT_COPY(ahrs_impl.ltp_to_imu_quat, *body_to_imu_quat); #ifdef IMU_MAG_OFFSET ahrs_impl.mag_offset = IMU_MAG_OFFSET; #else diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c index c676cb16fd..b486ed3161 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c @@ -117,7 +117,8 @@ void ahrs_init(void) { ahrs.status = AHRS_UNINIT; /* set ltp_to_imu so that body is zero */ - INT32_EULERS_OF_RMAT(ahrs_impl.ltp_to_imu_euler, imu.body_to_imu_rmat); + memcpy(&ahrs_impl.ltp_to_imu_euler, orientationGetEulers_i(&imu.body_to_imu), + sizeof(struct Int32Eulers)); INT_RATES_ZERO(ahrs_impl.imu_rate); INT_RATES_ZERO(ahrs_impl.gyro_bias); @@ -320,12 +321,13 @@ __attribute__ ((always_inline)) static inline void get_psi_measurement_from_mag( } /* Rotate angles and rates from imu to body frame and set state */ -__attribute__ ((always_inline)) static inline void set_body_state_from_euler(void) { +static void set_body_state_from_euler(void) { + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); struct Int32RMat ltp_to_imu_rmat, ltp_to_body_rmat; /* Compute LTP to IMU rotation matrix */ INT32_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler); /* Compute LTP to BODY rotation matrix */ - INT32_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, imu.body_to_imu_rmat); + INT32_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, *body_to_imu_rmat); /* Set state */ #ifdef AHRS_UPDATE_FW_ESTIMATOR struct Int32Eulers ltp_to_body_euler; @@ -339,7 +341,7 @@ __attribute__ ((always_inline)) static inline void set_body_state_from_euler(voi struct Int32Rates body_rate; /* compute body rates */ - INT32_RMAT_TRANSP_RATEMULT(body_rate, imu.body_to_imu_rmat, ahrs_impl.imu_rate); + INT32_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_impl.imu_rate); /* Set state */ stateSetBodyRates_i(&body_rate); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index d5e6aa3bc1..de7bd221df 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -197,7 +197,8 @@ void ahrs_init(void) { ahrs_impl.heading_aligned = FALSE; /* set ltp_to_imu so that body is zero */ - QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat); + memcpy(&ahrs_impl.ltp_to_imu_quat, orientationGetQuat_i(&imu.body_to_imu), + sizeof(struct Int32Quat)); INT_RATES_ZERO(ahrs_impl.imu_rate); INT_RATES_ZERO(ahrs_impl.gyro_bias); @@ -343,7 +344,8 @@ void ahrs_update_accel(void) { /* convert centrifucal acceleration from body to imu frame */ struct Int32Vect3 acc_c_imu; - INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body); + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); + INT32_RMAT_VMULT(acc_c_imu, *body_to_imu_rmat, acc_c_body); /* and subtract it from imu measurement to get a corrected measurement * of the gravity vector */ @@ -679,7 +681,8 @@ void ahrs_realign_heading(int32_t heading) { QUAT_COPY(ltp_to_body_quat, q); /* compute ltp to imu rotations */ - INT32_QUAT_COMP(ahrs_impl.ltp_to_imu_quat, ltp_to_body_quat, imu.body_to_imu_quat); + struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&imu.body_to_imu); + INT32_QUAT_COMP(ahrs_impl.ltp_to_imu_quat, ltp_to_body_quat, *body_to_imu_quat); /* Set state */ stateSetNedToBodyQuat_i(<p_to_body_quat); @@ -692,7 +695,8 @@ void ahrs_realign_heading(int32_t heading) { static inline void set_body_state_from_quat(void) { /* Compute LTP to BODY quaternion */ struct Int32Quat ltp_to_body_quat; - INT32_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat); + struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&imu.body_to_imu); + INT32_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, *body_to_imu_quat); /* Set state */ #ifdef AHRS_UPDATE_FW_ESTIMATOR struct Int32Eulers neutrals_to_body_eulers = { @@ -710,7 +714,8 @@ static inline void set_body_state_from_quat(void) { /* compute body rates */ struct Int32Rates body_rate; - INT32_RMAT_TRANSP_RATEMULT(body_rate, imu.body_to_imu_rmat, ahrs_impl.imu_rate); + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); + INT32_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_impl.imu_rate); /* Set state */ stateSetBodyRates_i(&body_rate); } diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index 4e28480169..c331c4eb94 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -26,6 +26,7 @@ #include BOARD_CONFIG #include "subsystems/imu.h" +#include "state.h" #ifdef IMU_POWER_GPIO #include "mcu_periph/gpio.h" @@ -130,17 +131,9 @@ INFO("Magnetometer neutrals are set to zero, you should calibrate!") INT_VECT3_ZERO(imu.mag_neutral); #endif - /* - Compute quaternion and rotation matrix - for conversions between body and imu frame - */ - 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(imu.body_to_imu_quat, body_to_imu_eulers); - INT32_QUAT_NORMALIZE(imu.body_to_imu_quat); - INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); + struct FloatEulers body_to_imu_eulers = + {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; + orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); @@ -163,13 +156,55 @@ INFO("Magnetometer neutrals are set to zero, you should calibrate!") void imu_float_init(void) { - /* - Compute quaternion and rotation matrix - for conversions between body and imu frame - */ - EULERS_ASSIGN(imuf.body_to_imu_eulers, - IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI); - FLOAT_QUAT_OF_EULERS(imuf.body_to_imu_quat, imuf.body_to_imu_eulers); - FLOAT_QUAT_NORMALIZE(imuf.body_to_imu_quat); - FLOAT_RMAT_OF_EULERS(imuf.body_to_imu_rmat, imuf.body_to_imu_eulers); + struct FloatEulers body_to_imu_eulers = + {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; + orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); } + +void imu_SetBodyToImuPhi(float phi) { + struct FloatEulers imu_to_body_eulers; + memcpy(&imu_to_body_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); + imu_to_body_eulers.phi = phi; + orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers); +} + +void imu_SetBodyToImuTheta(float theta) { + struct FloatEulers imu_to_body_eulers; + memcpy(&imu_to_body_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); + imu_to_body_eulers.theta = theta; + orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers); +} + +void imu_SetBodyToImuPsi(float psi) { + struct FloatEulers imu_to_body_eulers; + memcpy(&imu_to_body_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); + imu_to_body_eulers.psi = psi; + orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers); +} + +void imu_SetBodyToImuCurrent(float set) { + imu.b2i_set_current = set; + + if (imu.b2i_set_current) { + // adjust imu_to_body roll and pitch by current NedToBody roll and pitch + struct FloatEulers imu_to_body_eulers; + memcpy(&imu_to_body_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); + if (stateIsAttitudeValid()) { + // adjust imu_to_body roll and pitch by current NedToBody roll and pitch + imu_to_body_eulers.phi += stateGetNedToBodyEulers_f()->phi; + imu_to_body_eulers.theta += stateGetNedToBodyEulers_f()->theta; + orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers); + } + else { + // indicate that we couldn't set to current roll/pitch + imu.b2i_set_current = FALSE; + } + } + else { + // reset to BODY_TO_IMU as defined in airframe file + struct FloatEulers imu_to_body_eulers = + {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; + orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers); + } +} + diff --git a/sw/airborne/subsystems/imu.h b/sw/airborne/subsystems/imu.h index 01bda89c08..602661ba66 100644 --- a/sw/airborne/subsystems/imu.h +++ b/sw/airborne/subsystems/imu.h @@ -29,6 +29,7 @@ #include "math/pprz_algebra_int.h" #include "math/pprz_algebra_float.h" +#include "math/pprz_orientation_conversion.h" #include "generated/airframe.h" /* must be defined by underlying hardware */ @@ -48,8 +49,12 @@ struct Imu { struct Int32Rates gyro_unscaled; ///< unscaled gyroscope measurements struct Int32Vect3 accel_unscaled; ///< unscaled accelerometer measurements struct Int32Vect3 mag_unscaled; ///< unscaled magnetometer measurements - struct Int32Quat body_to_imu_quat; ///< rotation from body to imu frame as a unit quaternion - struct Int32RMat body_to_imu_rmat; ///< rotation from body to imu frame as a rotation matrix + struct OrientationReps body_to_imu; ///< rotation from body to imu frame + + /** flag for adjusting body_to_imu via settings. + * if FALSE, reset to airframe values, if TRUE set current roll/pitch + */ + bool_t b2i_set_current; }; /** abstract IMU interface providing floating point interface */ @@ -58,9 +63,7 @@ struct ImuFloat { struct FloatVect3 accel; struct FloatVect3 mag; struct FloatRates gyro_prev; - struct FloatEulers body_to_imu_eulers; - struct FloatQuat body_to_imu_quat; - struct FloatRMat body_to_imu_rmat; + struct OrientationReps body_to_imu; ///< rotation from body to imu frame uint32_t sample_count; }; @@ -77,6 +80,11 @@ extern struct ImuFloat imuf; extern void imu_init(void); extern void imu_float_init(void); +extern void imu_SetBodyToImuPhi(float phi); +extern void imu_SetBodyToImuTheta(float theta); +extern void imu_SetBodyToImuPsi(float psi); +extern void imu_SetBodyToImuCurrent(float set); +extern void imu_ResetBodyToImu(float reset); #if !defined IMU_BODY_TO_IMU_PHI && !defined IMU_BODY_TO_IMU_THETA && !defined IMU_BODY_TO_IMU_PSI #define IMU_BODY_TO_IMU_PHI 0 diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c index b4735289b8..ff70fef84a 100644 --- a/sw/airborne/subsystems/ins/hf_float.c +++ b/sw/airborne/subsystems/ins/hf_float.c @@ -463,7 +463,8 @@ void b2_hff_propagate(void) { /* rotate imu accel measurement to body frame and filter */ struct Int32Vect3 acc_meas_body; - INT32_RMAT_TRANSP_VMULT(acc_meas_body, imu.body_to_imu_rmat, imu.accel); + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); + INT32_RMAT_TRANSP_VMULT(acc_meas_body, *body_to_imu_rmat, imu.accel); struct Int32Vect3 acc_body_filtered; acc_body_filtered.x = update_butterworth_2_low_pass_int(&filter_x, acc_meas_body.x); diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index 99b73b36bf..8256e3aa37 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -357,10 +357,11 @@ void ahrs_propagate(void) { // fill command vector struct Int32Rates gyro_meas_body; - INT32_RMAT_TRANSP_RATEMULT(gyro_meas_body, imu.body_to_imu_rmat, imu.gyro); + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); + INT32_RMAT_TRANSP_RATEMULT(gyro_meas_body, *body_to_imu_rmat, imu.gyro); RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body); struct Int32Vect3 accel_meas_body; - INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); + INT32_RMAT_TRANSP_VMULT(accel_meas_body, *body_to_imu_rmat, imu.accel); ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body); // update correction gains diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 74f2a5b054..d2eafd4f1a 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -220,7 +220,8 @@ void ins_reset_altitude_ref(void) { void ins_propagate(void) { /* untilt accels */ struct Int32Vect3 accel_meas_body; - INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); + INT32_RMAT_TRANSP_VMULT(accel_meas_body, *body_to_imu_rmat, imu.accel); struct Int32Vect3 accel_meas_ltp; INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, (*stateGetNedToBodyRMat_i()), accel_meas_body);