[ins] float_invariant has own body_to_imu

sw/airborne/subsystems/ins/ins_float_invariant.c

@@ -33,7 +33,6 @@
 
 #include "subsystems/ins.h"
 #include "subsystems/gps.h"
-#include "subsystems/imu.h"
 
 #include "generated/airframe.h"
 #include "generated/flight_plan.h"
@@ -312,7 +311,7 @@ void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* a
 
   // fill command vector
   struct Int32Rates gyro_meas_body;
-  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
+  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ins_float_inv.body_to_imu);
   int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro);
   RATES_FLOAT_OF_BFP(ins_float_inv.cmd.rates, gyro_meas_body);
   struct Int32Vect3 accel_meas_body;
@@ -503,7 +502,7 @@ void ins_float_invariant_update_mag(struct Int32Vect3* mag)
     }
   } else {
     // values are moving
-    struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
+    struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ins_float_inv.body_to_imu);
     struct Int32Vect3 mag_meas_body;
     // new values in body frame
     int32_rmat_transp_vmult(&mag_meas_body, body_to_imu_rmat, mag);
@@ -686,3 +685,13 @@ void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
   VECT3_ADD(v2, v1);
   QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
 }
+
+void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
+{
+  orientationSetQuat_f(&ins_float_inv.body_to_imu, q_b2i);
+
+  if (!ins_float_inv.is_aligned) {
+    /* Set ltp_to_imu so that body is zero */
+    memcpy(&ins_float_inv.state.quat, q_b2i, sizeof(struct FloatQuat));
+  }
+}

sw/airborne/subsystems/ins/ins_float_invariant.h

@@ -111,6 +111,9 @@ struct InsFloatInv {
 
   bool_t reset;                       ///< flag to request reset/reinit the filter
 
+  /** body_to_imu rotation */
+  struct OrientationReps body_to_imu;
+
   struct FloatVect3 mag_h;
   bool_t is_aligned;
 };
@@ -119,6 +122,7 @@ extern struct InsFloatInv ins_float_inv;
 
 extern void ins_float_inv_init(void);
 extern void ins_float_inv_update_gps(void);
+extern void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i);
 
 extern void ins_float_invariant_propagate(struct Int32Rates* gyro,
                                           struct Int32Vect3* accel, float dt);

sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c

@@ -76,6 +76,7 @@ static abi_event baro_ev;
 static abi_event mag_ev;
 static abi_event gyro_ev;
 static abi_event aligner_ev;
+static abi_event body_to_imu_ev;
 static abi_event geo_mag_ev;
 
 static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
@@ -116,6 +117,12 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
   }
 }
 
+static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
+                           struct FloatQuat *q_b2i_f)
+{
+  ins_float_inv_set_body_to_imu_quat(q_b2i_f);
+}
+
 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
 {
   memcpy(&ins_float_inv.mag_h, h, sizeof(struct FloatVect3));
@@ -131,6 +138,7 @@ void ins_float_inv_register(void)
   AbiBindMsgIMU_MAG_INT32(INS_FINV_MAG_ID, &mag_ev, mag_cb);
   AbiBindMsgIMU_GYRO_INT32(INS_FINV_IMU_ID, &gyro_ev, gyro_cb);
   AbiBindMsgIMU_LOWPASSED(INS_FINV_IMU_ID, &aligner_ev, aligner_cb);
+  AbiBindMsgBODY_TO_IMU_QUAT(INS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
   AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
 
 #if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM