Merge pull request #783 from flixr/body_to_imu

[imu] body_to_imu: use generic orientation representation and expose it in settings to change during operation

You can now change the body_to_imu orientation via settings from the GCS, which should make experimentally "tuning" this for nice hover easier.
Also added setting "b2i curr roll/pitch" to set body_to_imu to current roll/pitch (on TRUE) and reset it to airframe file values on FALSE.

conf/settings/estimation/body_to_imu.xml

@@ -0,0 +1,12 @@
+<!DOCTYPE settings SYSTEM "../settings.dtd">
+
+<settings>
+  <dl_settings>
+    <dl_settings NAME="body2imu">
+      <dl_setting MAX="90" MIN="-90" STEP="0.5" VAR="imu.body_to_imu.eulers_f.phi" shortname="b2i phi" module="subsystems/imu" param="IMU_BODY_TO_IMU_PHI" unit="rad" alt_unit="deg" handler="SetBodyToImuPhi"/>
+      <dl_setting MAX="90" MIN="-90" STEP="0.5" VAR="imu.body_to_imu.eulers_f.theta" shortname="b2i theta" module="subsystems/imu" param="IMU_BODY_TO_IMU_THETA" unit="rad" alt_unit="deg" handler="SetBodyToImuTheta"/>
+      <dl_setting MAX="180" MIN="-180" STEP="0.5" VAR="imu.body_to_imu.eulers_f.psi" shortname="b2i psi" module="subsystems/imu" param="IMU_BODY_TO_IMU_PSI" unit="rad" alt_unit="deg" handler="SetBodyToImuPsi"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="imu.b2i_set_current" values="FALSE|TRUE" shortname="b2i cur roll/pitch" module="subsystems/imu" handler="SetBodyToImuCurrent"/>
+    </dl_settings>
+  </dl_settings>
+</settings>

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;

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)

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

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;

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;

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;

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(&ltp_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);
 

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;

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

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

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(&ltp_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);
 }

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

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

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

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

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