[ahrs] use ABI for body_to_imu

conf/abi.xml

@@ -42,6 +42,10 @@
       <field name="mag" type="struct Int32Vect3"/>
     </message>
 
+    <message name="BODY_TO_IMU_QUAT" id="8">
+      <field name="q_b2i_f" type="struct FloatQuat"/>
+    </message>
+
   </msg_class>
 
 

sw/airborne/firmwares/rotorcraft/main.c

@@ -174,6 +174,9 @@ STATIC_INLINE void main_init(void)
 
   DefaultAhrsRegister();
 
+  // send body_to_imu from here for now
+  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
+
 #if USE_GPS
   gps_init();
 #endif

sw/airborne/subsystems/ahrs.c

@@ -27,8 +27,6 @@
 
 #include "subsystems/ahrs.h"
 #include "subsystems/imu.h"
-#include "subsystems/abi.h"
-#include "mcu_periph/sys_time.h"
 
 struct Ahrs ahrs;
 
@@ -37,16 +35,7 @@ void ahrs_register_impl(AhrsInit init, AhrsUpdateGps update_gps)
   ahrs.init = init;
   ahrs.update_gps = update_gps;
 
-  // TODO: remove hacks
-#if !USE_IMU
-  struct OrientationReps body_to_imu;
-  struct FloatEulers eulers_zero = {0., 0., 0.};
-  orientationSetEulers_f(&body_to_imu, &eulers_zero);
-  ahrs.init(&body_to_imu);
-#elif !defined SITL || USE_NPS
-  /* call init function of implementation */
-  ahrs.init(&imu.body_to_imu);
-#endif
+  ahrs.init();
 
   ahrs.status = AHRS_REGISTERED;
 }

sw/airborne/subsystems/ahrs.h

@@ -41,7 +41,7 @@
 #include AHRS_TYPE_H
 #endif
 
-typedef void (*AhrsInit)(struct OrientationReps* body_to_imu);
+typedef void (*AhrsInit)(void);
 typedef void (*AhrsUpdateGps)(void);
 
 /** Attitude and Heading Reference System state */

sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c

@@ -57,17 +57,12 @@ static inline void set_body_state_from_quat(void);
 struct AhrsMlkf ahrs_mlkf;
 
 
-void ahrs_mlkf_init(struct OrientationReps* body_to_imu) {
-
-  /* save body_to_imu */
-  ahrs_mlkf_set_body_to_imu(body_to_imu);
+void ahrs_mlkf_init(void) {
 
   ahrs_mlkf.status = AHRS_MLKF_UNINIT;
 
-  /* Set ltp_to_imu so that body is zero */
-  memcpy(&ahrs_mlkf.ltp_to_imu_quat, orientationGetQuat_f(&ahrs_mlkf.body_to_imu),
-         sizeof(struct FloatQuat));
-
+  /* init ltp_to_imu quaternion as zero/identity rotation */
+  float_quat_identity(&ahrs_mlkf.ltp_to_imu_quat);
   FLOAT_RATES_ZERO(ahrs_mlkf.imu_rate);
 
   VECT3_ASSIGN(ahrs_mlkf.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
@@ -91,9 +86,21 @@ void ahrs_mlkf_init(struct OrientationReps* body_to_imu) {
 
 void ahrs_mlkf_set_body_to_imu(struct OrientationReps* body_to_imu)
 {
-  orientationSetQuat_f(&ahrs_mlkf.body_to_imu, orientationGetQuat_f(body_to_imu));
+  ahrs_mlkf_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu));
 }
 
+void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat* q_b2i)
+{
+  orientationSetQuat_f(&ahrs_mlkf.body_to_imu, q_b2i);
+
+  if (ahrs_mlkf.status == AHRS_MLKF_UNINIT) {
+    /* Set ltp_to_imu so that body is zero */
+    memcpy(&ahrs_mlkf.ltp_to_imu_quat, orientationGetQuat_f(&ahrs_mlkf.body_to_imu),
+           sizeof(struct FloatQuat));
+  }
+}
+
+
 bool_t ahrs_mlkf_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
                        struct Int32Vect3* lp_mag)
 {

sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h

@@ -62,8 +62,9 @@ struct AhrsMlkf {
 
 extern struct AhrsMlkf ahrs_mlkf;
 
-extern void ahrs_mlkf_init(struct OrientationReps* body_to_imu);
+extern void ahrs_mlkf_init(void);
 extern void ahrs_mlkf_set_body_to_imu(struct OrientationReps* body_to_imu);
+extern void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat* q_b2i);
 extern bool_t ahrs_mlkf_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
                               struct Int32Vect3* lp_mag);
 extern void ahrs_mlkf_propagate(struct Int32Rates* gyro, float dt);

sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c

@@ -48,8 +48,9 @@ static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) {
 static abi_event gyro_ev;
 static abi_event accel_ev;
 static abi_event mag_ev;
-
 static abi_event aligner_ev;
+static abi_event body_to_imu_ev;
+
 
 static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
                     const struct Int32Rates* gyro)
@@ -103,6 +104,12 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
   }
 }
 
+static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
+                           const struct FloatQuat* q_b2i_f)
+{
+  ahrs_mlkf_set_body_to_imu_quat((struct FloatQuat*)q_b2i_f);
+}
+
 void ahrs_mlkf_register(void)
 {
   ahrs_register_impl(ahrs_mlkf_init, NULL);
@@ -114,6 +121,7 @@ void ahrs_mlkf_register(void)
   AbiBindMsgIMU_ACCEL_INT32(AHRS_MLKF_IMU_ID, &accel_ev, accel_cb);
   AbiBindMsgIMU_MAG_INT32(AHRS_MLKF_IMU_ID, &mag_ev, mag_cb);
   AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
+  AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
 
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);