[ahrs] start using ABI for imu data

conf/abi.xml

@@ -16,6 +16,22 @@
       <field name="distance" type="float" unit="m"/>
     </message>
 
+
+    <message name="IMU_GYRO_INT32" id="4">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="gyro" type="struct Int32Rates"/>
+    </message>
+
+    <message name="IMU_ACCEL_INT32" id="5">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="accel" type="struct Int32Vect3"/>
+    </message>
+
+    <message name="IMU_MAG_INT32" id="6">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="mag" type="struct Int32Vect3"/>
+    </message>
+
   </msg_class>
 
 

conf/modules/ahrs_chimu_spi.xml

@@ -20,7 +20,7 @@
     <file name="imu_chimu.c"/>
     <file name="ahrs.c" dir="subsystems"/>
     <raw>
-      ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_module.h\"
+      ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ahrs_chimu.h\"
     </raw>
   </makefile>
 </module>

conf/modules/ahrs_chimu_uart.xml

@@ -24,7 +24,7 @@
     <file name="imu_chimu.c"/>
     <file name="ahrs.c" dir="subsystems"/>
     <raw>
-      ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_module.h\"
+      ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ahrs_chimu.h\"
     </raw>
   </makefile>
 </module>

conf/settings/estimation/ahrs_float_mlkf.xml

@@ -4,9 +4,9 @@
   <dl_settings>
 
     <dl_settings NAME="AHRS">
-       <dl_setting var="ahrs_impl.mag_noise.x" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_x" param="AHRS_MAG_NOISE_X"/>
+       <dl_setting var="ahrs_mlkf.mag_noise.x" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_x" param="AHRS_MAG_NOISE_X"/>
-       <dl_setting var="ahrs_impl.mag_noise.y" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_y" param="AHRS_MAG_NOISE_Y"/>
+       <dl_setting var="ahrs_mlkf.mag_noise.y" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_y" param="AHRS_MAG_NOISE_Y"/>
-       <dl_setting var="ahrs_impl.mag_noise.z" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_z" param="AHRS_MAG_NOISE_Z"/>
+       <dl_setting var="ahrs_mlkf.mag_noise.z" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_z" param="AHRS_MAG_NOISE_Z"/>
     </dl_settings>
 
   </dl_settings>

sw/airborne/arch/sim/sim_ahrs.c

@@ -15,7 +15,6 @@ float sim_psi;    ///< in radians
 float sim_p;      ///< in radians/s
 float sim_q;      ///< in radians/s
 float sim_r;      ///< in radians/s
-bool_t ahrs_sim_available;
 
 // Updates from Ocaml sim
 
@@ -24,8 +23,6 @@ value provide_attitude(value phi, value theta, value psi) {
   sim_theta = Double_val(theta);
   sim_psi = - Double_val(psi) + M_PI/2.;
 
-  ahrs_sim_available = TRUE;
-
   return Val_unit;
 }
 
@@ -34,8 +31,6 @@ value provide_rates(value p, value q, value r) {
   sim_q = Double_val(q);
   sim_r = Double_val(r);
 
-  ahrs_sim_available = TRUE;
-
   return Val_unit;
 }
 

sw/airborne/boards/ardrone/actuators_at.c

@@ -41,21 +41,21 @@ void actuators_set(pprz_t commands[]) {
   float yaw = ((float)commands[COMMAND_YAW] / (float)MAX_PPRZ);
 
   //Starting engine
-  if(thrust > 0 && (ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT || ahrs_impl.control_state == CTRL_LANDED))
+  if(thrust > 0 && (ahrs_ardrone2.control_state == CTRL_DEFAULT || ahrs_ardrone2.control_state == CTRL_INIT || ahrs_ardrone2.control_state == CTRL_LANDED))
     at_com_send_ref(REF_TAKEOFF);
 
   //Check emergency or stop engine
-  if((ahrs_impl.state & ARDRONE_EMERGENCY_MASK) != 0)
+  if((ahrs_ardrone2.state & ARDRONE_EMERGENCY_MASK) != 0)
     at_com_send_ref(REF_EMERGENCY);
-  else if(thrust < -0.9 && !(ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT || ahrs_impl.control_state == CTRL_LANDED))
+  else if(thrust < -0.9 && !(ahrs_ardrone2.control_state == CTRL_DEFAULT || ahrs_ardrone2.control_state == CTRL_INIT || ahrs_ardrone2.control_state == CTRL_LANDED))
     at_com_send_ref(0);
 
   //Calibration
-  if((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 && (ahrs_impl.control_state == CTRL_FLYING || ahrs_impl.control_state == CTRL_HOVERING))
+  if((ahrs_ardrone2.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 && (ahrs_ardrone2.control_state == CTRL_FLYING || ahrs_ardrone2.control_state == CTRL_HOVERING))
     at_com_send_calib(0);
 
   //Moving
-  if((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 && (ahrs_impl.control_state == CTRL_FLYING || ahrs_impl.control_state == CTRL_HOVERING))
+  if((ahrs_ardrone2.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 && (ahrs_ardrone2.control_state == CTRL_FLYING || ahrs_ardrone2.control_state == CTRL_HOVERING))
     at_com_send_pcmd(1, thrust, roll, pitch, yaw);
 
   //Keep alive (FIXME)

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -128,6 +128,11 @@ PRINT_CONFIG_VAR(MODULES_FREQUENCY)
 PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
 #endif
 
+
+#define __DefaultAhrsRegister(_x) _x ## _register()
+#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x)
+#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl)
+
 #if USE_AHRS && USE_IMU
 
 #ifdef AHRS_PROPAGATE_FREQUENCY
@@ -137,10 +142,6 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
 #endif
 #endif
 
-#define __DefaultAhrsRegister(_x) _x ## _register()
-#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x)
-#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl)
-
 static inline void on_gyro_event( void );
 static inline void on_accel_event( void );
 static inline void on_mag_event( void );
@@ -202,9 +203,13 @@ void init_ap( void ) {
 #endif
 
 #if USE_AHRS
+#if defined SITL && !USE_NPS && !USE_INFRARED
+  ahrs_sim_init();
+#else
   ahrs_init();
   DefaultAhrsRegister();
 #endif
+#endif
 
 #if USE_AHRS && USE_IMU
   register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
@@ -602,9 +607,8 @@ void sensors_task( void ) {
 #endif // USE_IMU
 
   //FIXME: this is just a kludge
-#if USE_AHRS && defined SITL && !USE_NPS
+#if USE_AHRS && defined SITL && !USE_NPS && !USE_INFRARED
-  // dt is not really used in ahrs_sim
+  update_ahrs_from_sim();
-  ahrs_propagate(&imu.gyro, 1./PERIODIC_FREQUENCY);
 #endif
 
 #if USE_GPS
@@ -726,51 +730,43 @@ static inline void on_gps_solution( void ) {
 
 #if USE_AHRS
 #if USE_IMU
-static inline void on_accel_event( void ) {
+static inline void on_accel_event(void)
-#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
+{
-PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.")
-  // timestamp in usec when last callback was received
-  static uint32_t last_ts = 0;
   // current timestamp
   uint32_t now_ts = get_sys_time_usec();
-  // dt between this and last callback in seconds
-  float dt = (float)(now_ts - last_ts) / 1e6;
-  last_ts = now_ts;
-#else
-PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS accel update.")
-PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
-  const float dt = 1./AHRS_CORRECT_FREQUENCY;
-#endif
 
   imu_scale_accel(&imu);
-  if (ahrs.status == AHRS_RUNNING) {
+
-    ahrs_update_accel(&imu.accel, dt);
+  AbiSendMsgIMU_ACCEL_INT32(1, &now_ts, &imu.accel);
-  }
 }
 
-static inline void on_gyro_event( void ) {
+static inline void on_gyro_event(void)
-#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
+{
-PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.")
-  // timestamp in usec when last callback was received
-  static uint32_t last_ts = 0;
   // current timestamp
   uint32_t now_ts = get_sys_time_usec();
+
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
+  PRINT_CONFIG_MSG("Calculating dt for INS propagation.")
+    // timestamp in usec when last callback was received
+    static uint32_t last_ts = 0;
   // dt between this and last callback in seconds
   float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
 #else
-PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS/INS propagation.")
+  PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for INS propagation.")
-PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
+    PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
-  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
+    const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
 #endif
 
   ahrs_timeout_counter = 0;
 
   imu_scale_gyro(&imu);
 
+  AbiSendMsgIMU_GYRO_INT32(1, &now_ts, &imu.gyro_prev);
+
 #if USE_AHRS_ALIGNER
   // Run aligner on raw data as it also makes averages.
-  if (ahrs.status == AHRS_REGISTERED) {
+  if (ahrs.status != AHRS_RUNNING) {
     ahrs_aligner_run();
     if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
       if (ahrs_align(&ahrs_aligner.lp_gyro, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag)) {
@@ -781,8 +777,6 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
   }
 #endif
 
-  ahrs_propagate(&imu.gyro_prev, dt);
-
 #if defined SITL && USE_NPS
   if (nps_bypass_ahrs) sim_overwrite_ahrs();
 #endif
@@ -796,25 +790,11 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
 static inline void on_mag_event(void)
 {
 #if USE_MAGNETOMETER
-#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
-PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.")
-  // timestamp in usec when last callback was received
-  static uint32_t last_ts = 0;
   // current timestamp
   uint32_t now_ts = get_sys_time_usec();
-  // dt between this and last callback in seconds
-  float dt = (float)(now_ts - last_ts) / 1e6;
-  last_ts = now_ts;
-#else
-PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.")
-PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
-  const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
-#endif
 
   imu_scale_mag(&imu);
-  if (ahrs.status == AHRS_RUNNING) {
+  AbiSendMsgIMU_MAG_INT32(1, &now_ts, &imu.mag);
-    ahrs_update_mag(&imu.mag, dt);
-  }
 #endif
 }
 

sw/airborne/firmwares/rotorcraft/main.c

@@ -69,7 +69,9 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO
 #include "firmwares/rotorcraft/guidance.h"
 
 #include "subsystems/ahrs.h"
+#if USE_AHRS_ALIGNER
 #include "subsystems/ahrs/ahrs_aligner.h"
+#endif
 #include "subsystems/ins.h"
 
 #include "state.h"
@@ -161,7 +163,9 @@ STATIC_INLINE void main_init( void ) {
   baro_init();
 #endif
   imu_init();
+#if USE_AHRS_ALIGNER
   ahrs_aligner_init();
+#endif
   ahrs_init();
   ins_init();
 
@@ -313,61 +317,53 @@ STATIC_INLINE void main_event( void ) {
 }
 
 static inline void on_accel_event( void ) {
-#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
-PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.")
-  // timestamp in usec when last callback was received
-  static uint32_t last_ts = 0;
   // current timestamp
   uint32_t now_ts = get_sys_time_usec();
-  // dt between this and last callback
-  float dt = (float)(now_ts - last_ts) / 1e6;
-  last_ts = now_ts;
-#else
-PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS accel update.")
-PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
-  const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
-#endif
 
   imu_scale_accel(&imu);
 
-  if (ahrs.status == AHRS_RUNNING) {
+  AbiSendMsgIMU_ACCEL_INT32(1, &now_ts, &imu.accel);
-    ahrs_update_accel(&imu.accel, dt);
-  }
 }
 
 static inline void on_gyro_event( void ) {
-#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
-PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.")
-  // timestamp in usec when last callback was received
-  static uint32_t last_ts = 0;
   // current timestamp
   uint32_t now_ts = get_sys_time_usec();
+
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for INS propagation.")
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
   // dt between this and last callback in seconds
   float dt = (float)(now_ts - last_ts) / 1e6;
   last_ts = now_ts;
 #else
-PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS/INS propagation.")
+PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for INS propagation.")
 PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
   const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
 #endif
 
   imu_scale_gyro(&imu);
 
-  if (ahrs.status == AHRS_REGISTERED) {
+  AbiSendMsgIMU_GYRO_INT32(1, &now_ts, &imu.gyro_prev);
+
+#if USE_AHRS_ALIGNER
+  if (ahrs.status != AHRS_RUNNING) {
     ahrs_aligner_run();
     if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
       if (ahrs_align(&ahrs_aligner.lp_gyro, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag)) {
         ahrs.status = AHRS_RUNNING;
       }
     }
+    return;
   }
-  else {
-    ahrs_propagate(&imu.gyro_prev, dt);
-#ifdef SITL
-    if (nps_bypass_ahrs) sim_overwrite_ahrs();
 #endif
-    ins_propagate(dt);
+
-  }
+#ifdef SITL
+  if (nps_bypass_ahrs) sim_overwrite_ahrs();
+#endif
+
+  ins_propagate(dt);
+
 #ifdef USE_VEHICLE_INTERFACE
   vi_notify_imu_available();
 #endif
@@ -384,27 +380,10 @@ static inline void on_gps_event(void) {
 
 static inline void on_mag_event(void) {
   imu_scale_mag(&imu);
-
-#if USE_MAGNETOMETER
-#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
-PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.")
-  // timestamp in usec when last callback was received
-  static uint32_t last_ts = 0;
   // current timestamp
   uint32_t now_ts = get_sys_time_usec();
-  // dt between this and last callback in seconds
-  float dt = (float)(now_ts - last_ts) / 1e6;
-  last_ts = now_ts;
-#else
-PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.")
-PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
-  const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
-#endif
 
-  if (ahrs.status == AHRS_RUNNING) {
+  AbiSendMsgIMU_MAG_INT32(1, &now_ts, &imu.mag);
-    ahrs_update_mag(&imu.mag, dt);
-  }
-#endif // USE_MAGNETOMETER
 
 #ifdef USE_VEHICLE_INTERFACE
   vi_notify_mag_available();

sw/airborne/modules/ins/ahrs_chimu.h

@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2014 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file modules/ins/ahrs_chimu.h
+ */
+
+#ifndef AHRS_CHIMU_H
+#define AHRS_CHIMU_H
+
+#include "modules/ins/ins_module.h"
+#include "subsystems/ahrs.h"
+
+#define DefaultAhrsImpl ahrs_chimu
+
+extern void ahrs_chimu_register(void);
+extern void ahrs_chimu_init(struct OrientationReps* body_to_imu);
+
+#endif

sw/airborne/modules/ins/ahrs_chimu_spi.c

@@ -26,6 +26,7 @@
 
 #include "ins_module.h"
 #include "imu_chimu.h"
+#include "ahrs_chimu.h"
 
 #include "led.h"
 
@@ -34,7 +35,14 @@ CHIMU_PARSER_DATA CHIMU_DATA;
 INS_FORMAT ins_roll_neutral;
 INS_FORMAT ins_pitch_neutral;
 
-void ahrs_init(void)
+void ahrs_chimu_update_gps(void);
+
+void ahrs_chimu_register(void)
+{
+  ahrs_register_impl(ahrs_chimu_init, NULL, ahrs_chimu_update_gps);
+}
+
+void ahrs_chimu_init(struct OrientationReps* body_to_imu __attribute__((unused)))
 {
   ahrs.status = AHRS_UNINIT;
 
@@ -68,10 +76,6 @@ void ahrs_init(void)
   InsSend(rate,12);
 }
 
-void ahrs_align(void)
-{
-  ahrs.status = AHRS_RUNNING;
-}
 
 void parse_ins_msg( void )
 {
@@ -98,6 +102,8 @@ void parse_ins_msg( void )
           0.
         }; // FIXME rate r
         stateSetBodyRates_f(&rates);
+        //FIXME
+        ahrs.status = AHRS_RUNNING;
       }
       else if(CHIMU_DATA.m_MsgID==0x02) {
 #if CHIMU_DOWNLINK_IMMEDIATE
@@ -109,7 +115,7 @@ void parse_ins_msg( void )
 }
 
 
-void ahrs_update_gps(void)
+void ahrs_chimu_update_gps(void)
 {
   // Send SW Centripetal Corrections
   uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02,   0x00, 0x00, 0x00, 0x00,   0x00, 0x00, 0x00, 0x00,   0x00, 0x00, 0x00, 0x00,   0xc2 };
@@ -130,9 +136,3 @@ void ahrs_update_gps(void)
 
   // Downlink Send
 }
-
-void ahrs_propagate(float dt __attribute__((unused))) {
-}
-
-void ahrs_update_accel(float dt __attribute__((unused))) {
-}

sw/airborne/modules/ins/ahrs_chimu_uart.c

@@ -30,7 +30,12 @@ CHIMU_PARSER_DATA CHIMU_DATA;
 INS_FORMAT ins_roll_neutral;
 INS_FORMAT ins_pitch_neutral;
 
-void ahrs_init(void)
+void ahrs_chimu_register(void)
+{
+  ahrs_register_impl(ahrs_chimu_init, NULL, NULL);
+}
+
+void ahrs_chimu_init(struct OrientationReps* body_to_imu __attribute__((unused)))
 {
   ahrs.status = AHRS_UNINIT;
 
@@ -61,10 +66,6 @@ void ahrs_init(void)
   CHIMU_Checksum(rate,12);
   InsSend(rate,12);
 }
-void ahrs_align(void)
-{
-  ahrs.status = AHRS_RUNNING;
-}
 
 
 void parse_ins_msg( void )
@@ -86,6 +87,7 @@ void parse_ins_msg( void )
           CHIMU_DATA.m_attitude.euler.psi
         };
         stateSetNedToBodyEulers_f(&att);
+        ahrs.status = AHRS_RUNNING;
 #if CHIMU_DOWNLINK_IMMEDIATE
         DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
 #endif
@@ -94,7 +96,3 @@ void parse_ins_msg( void )
     }
   }
 }
-
-void ahrs_update_gps( void )
-{
-}

sw/airborne/subsystems/ahrs.c

@@ -27,22 +27,28 @@
 
 #include "subsystems/ahrs.h"
 #include "subsystems/imu.h"
+#include "subsystems/abi.h"
+#include "mcu_periph/sys_time.h"
 
 struct Ahrs ahrs;
 
-void ahrs_register_impl(AhrsInit init, AhrsAlign align, AhrsPropagate propagate,
+void ahrs_register_impl(AhrsInit init, AhrsAlign align,
-                        AhrsUpdateAccel update_acc, AhrsUpdateMag update_mag,
                         AhrsUpdateGps update_gps)
 {
   ahrs.init = init;
   ahrs.align = align;
-  ahrs.propagate = propagate;
-  ahrs.update_accel = update_acc;
-  ahrs.update_mag = update_mag;
   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.status = AHRS_REGISTERED;
 }
@@ -52,9 +58,6 @@ void ahrs_init(void)
   ahrs.status = AHRS_UNINIT;
   ahrs.init = NULL;
   ahrs.align = NULL;
-  ahrs.propagate = NULL;
-  ahrs.update_accel = NULL;
-  ahrs.update_mag = NULL;
   ahrs.update_gps = NULL;
 }
 
@@ -67,26 +70,7 @@ bool_t ahrs_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
   return FALSE;
 }
 
-void ahrs_propagate(struct Int32Rates* gyro, float dt)
-{
-  if (ahrs.propagate != NULL && ahrs.status == AHRS_RUNNING) {
-    ahrs.propagate(gyro, dt);
-  }
-}
 
-void ahrs_update_accel(struct Int32Vect3* accel, float dt)
-{
-  if (ahrs.update_accel != NULL && ahrs.status == AHRS_RUNNING) {
-    ahrs.update_accel(accel, dt);
-  }
-}
-
-void ahrs_update_mag(struct Int32Vect3* mag, float dt)
-{
-  if (ahrs.update_mag != NULL && ahrs.status == AHRS_RUNNING) {
-    ahrs.update_mag(mag, dt);
-  }
-}
 
 void ahrs_update_gps(void)
 {
@@ -94,3 +78,32 @@ void ahrs_update_gps(void)
     ahrs.update_gps();
   }
 }
+
+
+
+/*
+ * REMOVE ME! keep temporarily for some test firmware
+ */
+void ahrs_propagate(struct Int32Rates* gyro)
+{
+  if (ahrs.status == AHRS_RUNNING) {
+    uint32_t stamp = get_sys_time_usec();
+    AbiSendMsgIMU_GYRO_INT32(1, &stamp, gyro);
+  }
+}
+
+void ahrs_update_accel(struct Int32Vect3* accel)
+{
+  if (ahrs.status == AHRS_RUNNING) {
+    uint32_t stamp = get_sys_time_usec();
+    AbiSendMsgIMU_ACCEL_INT32(1, &stamp, accel);
+  }
+}
+
+void ahrs_update_mag(struct Int32Vect3* mag)
+{
+  if (ahrs.status == AHRS_RUNNING) {
+    uint32_t stamp = get_sys_time_usec();
+    AbiSendMsgIMU_MAG_INT32(1, &stamp, mag);
+  }
+}

sw/airborne/subsystems/ahrs.h

@@ -44,9 +44,9 @@
 typedef void (*AhrsInit)(struct OrientationReps* body_to_imu);
 typedef bool_t (*AhrsAlign)(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
                             struct Int32Vect3* lp_mag);
-typedef void (*AhrsPropagate)(struct Int32Rates* gyro, float dt);
+//typedef void (*AhrsPropagate)(struct Int32Rates* gyro, float dt);
-typedef void (*AhrsUpdateAccel)(struct Int32Vect3* accel, float dt);
+//typedef void (*AhrsUpdateAccel)(struct Int32Vect3* accel, float dt);
-typedef void (*AhrsUpdateMag)(struct Int32Vect3* mag, float dt);
+//typedef void (*AhrsUpdateMag)(struct Int32Vect3* mag, float dt);
 typedef void (*AhrsUpdateGps)(void);
 //typedef void (*AhrsUpdateGps)(struct Gps* gps);
 
@@ -57,17 +57,16 @@ struct Ahrs {
   /* function pointers to actual implementation, set by ahrs_register_impl */
   AhrsInit init;
   AhrsAlign align;
-  AhrsPropagate propagate;
+  //AhrsPropagate propagate;
-  AhrsUpdateAccel update_accel;
+  //AhrsUpdateAccel update_accel;
-  AhrsUpdateMag update_mag;
+  //AhrsUpdateMag update_mag;
   AhrsUpdateGps update_gps;
 };
 
 /** global AHRS state */
 extern struct Ahrs ahrs;
 
-extern void ahrs_register_impl(AhrsInit init, AhrsAlign align, AhrsPropagate propagate,
+extern void ahrs_register_impl(AhrsInit init, AhrsAlign align,
-                               AhrsUpdateAccel update_acc, AhrsUpdateMag update_mag,
                                AhrsUpdateGps update_gps);
 
 /** AHRS initialization. Called at startup.
@@ -85,23 +84,20 @@ extern bool_t ahrs_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel
 /** Propagation. Usually integrates the gyro rates to angles.
  * Calls implementation if registered.
  * @param gyro pointer to gyro measurement
- * @param dt time difference since last propagation in seconds
  */
-extern void ahrs_propagate(struct Int32Rates* gyro, float dt);
+extern void ahrs_propagate(struct Int32Rates* gyro);
 
 /** Update AHRS state with accerleration measurements.
  * Calls implementation if registered.
  * @param accel pointer to accelerometer measurement
- * @param dt time difference since last update in seconds
  */
-extern void ahrs_update_accel(struct Int32Vect3* accel, float dt);
+extern void ahrs_update_accel(struct Int32Vect3* accel);
 
 /** Update AHRS state with magnetometer measurements.
  * Calls implementation if registered.
  * @param mag pointer to magnetometer measurement
- * @param dt time difference since last update in seconds
  */
-extern void ahrs_update_mag(struct Int32Vect3* mag, float dt);
+extern void ahrs_update_mag(struct Int32Vect3* mag);
 
 /** Update AHRS state with GPS measurements.
  *  Calls implementation if registered.

sw/airborne/subsystems/ahrs/ahrs_ardrone2.c

@@ -42,7 +42,7 @@
 #include "subsystems/gps/gps_ardrone2.h"
 #endif
 
-struct AhrsARDrone ahrs_impl;
+struct AhrsARDrone ahrs_ardrone2;
 struct AhrsAligner ahrs_aligner;
 unsigned char buffer[4096]; //Packet buffer
 
@@ -51,23 +51,28 @@ unsigned char buffer[4096]; //Packet buffer
 
 static void send_ahrs_ad2(void) {
   DOWNLINK_SEND_AHRS_ARDRONE2(DefaultChannel, DefaultDevice,
-      &ahrs_impl.state,
+      &ahrs_ardrone2.state,
-      &ahrs_impl.control_state,
+      &ahrs_ardrone2.control_state,
-      &ahrs_impl.eulers.phi,
+      &ahrs_ardrone2.eulers.phi,
-      &ahrs_impl.eulers.theta,
+      &ahrs_ardrone2.eulers.theta,
-      &ahrs_impl.eulers.psi,
+      &ahrs_ardrone2.eulers.psi,
-      &ahrs_impl.speed.x,
+      &ahrs_ardrone2.speed.x,
-      &ahrs_impl.speed.y,
+      &ahrs_ardrone2.speed.y,
-      &ahrs_impl.speed.z,
+      &ahrs_ardrone2.speed.z,
-      &ahrs_impl.accel.x,
+      &ahrs_ardrone2.accel.x,
-      &ahrs_impl.accel.y,
+      &ahrs_ardrone2.accel.y,
-      &ahrs_impl.accel.z,
+      &ahrs_ardrone2.accel.z,
-      &ahrs_impl.altitude,
+      &ahrs_ardrone2.altitude,
-      &ahrs_impl.battery);
+      &ahrs_ardrone2.battery);
 }
 #endif
 
-void ahrs_init(void) {
+void ahrs_ardrone2_register(void)
+{
+  ahrs_register_impl(ahrs_ardrone2_init, NULL, NULL);
+}
+
+void ahrs_ardrone2_init(struct OrientationReps* body_to_imu __attribute__((unused))) {
   init_at_com();
 
   //Set navdata_demo to FALSE and flat trim the ar drone
@@ -81,10 +86,6 @@ void ahrs_init(void) {
 #endif
 }
 
-void ahrs_align(void) {
-
-}
-
 #ifdef ARDRONE2_DEBUG
 static void dump(const void *_b, size_t s) {
   const unsigned char *b = _b;
@@ -100,7 +101,7 @@ static void dump(const void *_b, size_t s) {
 }
 #endif
 
-void ahrs_propagate(float dt __attribute__((unused))) {
+void ahrs_ardrone2_propagate(void) {
   int l;
 
   //Recieve the main packet
@@ -122,7 +123,7 @@ void ahrs_propagate(float dt __attribute__((unused))) {
 #endif
 
   //Set the state
-  ahrs_impl.state = main_packet->ardrone_state;
+  ahrs_ardrone2.state = main_packet->ardrone_state;
 
   //Init the option
   navdata_option_t* navdata_option = (navdata_option_t*)&(main_packet->options[0]);
@@ -144,15 +145,15 @@ void ahrs_propagate(float dt __attribute__((unused))) {
       navdata_demo = (navdata_demo_t*) navdata_option;
 
       //Set the AHRS state
-      ahrs_impl.control_state = navdata_demo->ctrl_state >> 16;
+      ahrs_ardrone2.control_state = navdata_demo->ctrl_state >> 16;
-      ahrs_impl.eulers.phi = navdata_demo->phi;
+      ahrs_ardrone2.eulers.phi = navdata_demo->phi;
-      ahrs_impl.eulers.theta = navdata_demo->theta;
+      ahrs_ardrone2.eulers.theta = navdata_demo->theta;
-      ahrs_impl.eulers.psi = navdata_demo->psi;
+      ahrs_ardrone2.eulers.psi = navdata_demo->psi;
-      ahrs_impl.speed.x = navdata_demo->vx / 1000;
+      ahrs_ardrone2.speed.x = navdata_demo->vx / 1000;
-      ahrs_impl.speed.y = navdata_demo->vy / 1000;
+      ahrs_ardrone2.speed.y = navdata_demo->vy / 1000;
-      ahrs_impl.speed.z = navdata_demo->vz / 1000;
+      ahrs_ardrone2.speed.z = navdata_demo->vz / 1000;
-      ahrs_impl.altitude = navdata_demo->altitude / 10;
+      ahrs_ardrone2.altitude = navdata_demo->altitude / 10;
-      ahrs_impl.battery = navdata_demo->vbat_flying_percentage;
+      ahrs_ardrone2.battery = navdata_demo->vbat_flying_percentage;
 
       //Set the ned to body eulers
       struct FloatEulers angles;
@@ -168,7 +169,7 @@ void ahrs_propagate(float dt __attribute__((unused))) {
       navdata_phys_measures = (navdata_phys_measures_t*) navdata_option;
 
       //Set the AHRS accel state
-      INT32_VECT3_SCALE_2(ahrs_impl.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
+      INT32_VECT3_SCALE_2(ahrs_ardrone2.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
       break;
 #ifdef USE_GPS_ARDRONE2
     case 27: //NAVDATA_GPS
@@ -194,9 +195,3 @@ void ahrs_propagate(float dt __attribute__((unused))) {
     navdata_option = (navdata_option_t*) ((uint32_t)navdata_option + navdata_option->size);
   }
 }
-
-void ahrs_aligner_init(void) {
-}
-
-void ahrs_aligner_run(void) {
-}

sw/airborne/subsystems/ahrs/ahrs_ardrone2.h

@@ -34,6 +34,7 @@
 #include "subsystems/ahrs/ahrs_aligner.h"
 #include "std.h"
 #include "math/pprz_algebra_int.h"
+#include "math/pprz_geodetic_float.h"
 
 struct AhrsARDrone {
   uint32_t state;             // ARDRONE_STATES
@@ -45,6 +46,12 @@ struct AhrsARDrone {
   uint32_t battery;           // in percentage
   struct Int32Quat ltp_to_imu_quat;
 };
-extern struct AhrsARDrone ahrs_impl;
+extern struct AhrsARDrone ahrs_ardrone2;
+
+#define DefaultAhrsImpl ahrs_ardrone2
+
+extern void ahrs_ardrone2_register(void);
+extern void ahrs_ardrone2_init(struct OrientationReps* body_to_imu);
+extern void ahrs_ardrone2_propagate(void);
 
 #endif /* AHRS_ARDRONE2_H */

sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c

@@ -38,6 +38,7 @@
 #include "subsystems/gps.h"
 #endif
 #include "state.h"
+#include "subsystems/abi.h"
 
 //#include "../../test/pprz_algebra_print.h"
 
@@ -89,6 +90,50 @@ static inline void compute_body_orientation_and_rates(void);
 
 struct AhrsFloatCmpl ahrs_fc;
 
+
+/** ABI binding for IMU data.
+ * Used for gyro, accel and mag ABI messages.
+ */
+#ifndef AHRS_FC_IMU_ID
+#define AHRS_FC_IMU_ID ABI_BROADCAST
+#endif
+static abi_event gyro_ev;
+static abi_event accel_ev;
+static abi_event mag_ev;
+
+static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
+                    const struct Int32Rates* gyro)
+{
+  static uint32_t last_stamp = 0;
+  if (last_stamp > 0 && ahrs_fc.status == AHRS_FC_RUNNING) {
+    float dt = (float)(*stamp - last_stamp) * 1e-6;
+    ahrs_fc_propagate((struct Int32Rates*)gyro, dt);
+  }
+  last_stamp = *stamp;
+}
+
+static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
+                     const struct Int32Vect3* accel)
+{
+  static uint32_t last_stamp = 0;
+  if (last_stamp > 0 && ahrs_fc.status == AHRS_FC_RUNNING) {
+    float dt = (float)(*stamp - last_stamp) * 1e-6;
+    ahrs_fc_update_accel((struct Int32Vect3*)accel, dt);
+  }
+  last_stamp = *stamp;
+}
+
+static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
+                   const struct Int32Vect3* mag)
+{
+  static uint32_t last_stamp = 0;
+  if (last_stamp > 0 && ahrs_fc.status == AHRS_FC_RUNNING) {
+    float dt = (float)(*stamp - last_stamp) * 1e-6;
+    ahrs_fc_update_mag((struct Int32Vect3*)mag, dt);
+  }
+  last_stamp = *stamp;
+}
+
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
 
@@ -141,8 +186,7 @@ static void send_rmat(void) {
 
 void ahrs_fc_register(void)
 {
-  ahrs_register_impl(ahrs_fc_init, ahrs_fc_align, ahrs_fc_propagate,
+  ahrs_register_impl(ahrs_fc_init, ahrs_fc_align, ahrs_fc_update_gps);
-                     ahrs_fc_update_accel, ahrs_fc_update_mag, ahrs_fc_update_gps);
 }
 
 void ahrs_fc_init(struct OrientationReps* body_to_imu)
@@ -150,6 +194,8 @@ void ahrs_fc_init(struct OrientationReps* body_to_imu)
   /* save body_to_imu pointer */
   ahrs_fc.body_to_imu = body_to_imu;
 
+  ahrs_fc.status = AHRS_FC_UNINIT;
+
   ahrs_fc.ltp_vel_norm_valid = FALSE;
   ahrs_fc.heading_aligned = FALSE;
 
@@ -180,6 +226,13 @@ void ahrs_fc_init(struct OrientationReps* body_to_imu)
   ahrs_fc.accel_cnt = 0;
   ahrs_fc.mag_cnt = 0;
 
+  /*
+   * Subscribe to scaled IMU measurements and attach callbacks
+   */
+  AbiBindMsgIMU_GYRO_INT32(AHRS_FC_IMU_ID, &gyro_ev, gyro_cb);
+  AbiBindMsgIMU_ACCEL_INT32(AHRS_FC_IMU_ID, &accel_ev, accel_cb);
+  AbiBindMsgIMU_MAG_INT32(AHRS_FC_IMU_ID, &mag_ev, mag_cb);
+
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
   register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
@@ -211,6 +264,8 @@ bool_t ahrs_fc_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
   RATES_COPY(bias0, *lp_gyro);
   RATES_FLOAT_OF_BFP(ahrs_fc.gyro_bias, bias0);
 
+  ahrs_fc.status = AHRS_FC_RUNNING;
+
   return TRUE;
 }
 

sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h

@@ -32,6 +32,11 @@
 
 #include "std.h"
 
+enum AhrsFCStatus {
+  AHRS_FC_UNINIT,
+  AHRS_FC_RUNNING
+};
+
 struct AhrsFloatCmpl {
   struct FloatRates gyro_bias;
   struct FloatRates rate_correction;
@@ -62,6 +67,8 @@ struct AhrsFloatCmpl {
   uint16_t mag_cnt;   ///< number of propagations since last mag update
 
   struct OrientationReps* body_to_imu;
+
+  enum AhrsFCStatus status;
 };
 
 extern struct AhrsFloatCmpl ahrs_fc;

sw/airborne/subsystems/ahrs/ahrs_float_dcm.c

@@ -36,6 +36,7 @@
 #include "math/pprz_algebra_float.h"
 
 #include "state.h"
+#include "subsystems/abi.h"
 
 #if USE_GPS
 #include "subsystems/gps.h"
@@ -92,6 +93,46 @@ float imu_health = 0.;
 #endif
 
 
+/** ABI binding for IMU data.
+ * Used for gyro, accel and mag ABI messages.
+ */
+#ifndef AHRS_DCM_IMU_ID
+#define AHRS_DCM_IMU_ID ABI_BROADCAST
+#endif
+static abi_event gyro_ev;
+static abi_event accel_ev;
+static abi_event mag_ev;
+
+static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
+                    const struct Int32Rates* gyro)
+{
+  static uint32_t last_stamp = 0;
+  if (last_stamp > 0 && ahrs_dcm.status == AHRS_DCM_RUNNING) {
+    float dt = (float)(*stamp - last_stamp) * 1e-6;
+    ahrs_dcm_propagate((struct Int32Rates*)gyro, dt);
+  }
+  last_stamp = *stamp;
+}
+
+static void accel_cb(uint8_t sender_id __attribute__((unused)),
+                     const uint32_t* stamp __attribute__((unused)),
+                     const struct Int32Vect3* accel)
+{
+  if (ahrs_dcm.status == AHRS_DCM_RUNNING) {
+    ahrs_dcm_update_accel((struct Int32Vect3*)accel);
+  }
+}
+
+static void mag_cb(uint8_t sender_id __attribute__((unused)),
+                   const uint32_t* stamp __attribute__((unused)),
+                   const struct Int32Vect3* mag)
+{
+  if (ahrs_dcm.status == AHRS_DCM_RUNNING) {
+    ahrs_dcm_update_mag((struct Int32Vect3*)mag);
+  }
+}
+
+
 static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
 {
   for (int i=0; i<3; i++) {
@@ -103,8 +144,7 @@ static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
 
 void ahrs_dcm_register(void)
 {
-  ahrs_register_impl(ahrs_dcm_init, ahrs_dcm_align, ahrs_dcm_propagate,
+  ahrs_register_impl(ahrs_dcm_init, ahrs_dcm_align, ahrs_dcm_update_gps);
-                     ahrs_dcm_update_accel, ahrs_dcm_update_mag, ahrs_dcm_update_gps);
 }
 
 void ahrs_dcm_init(struct OrientationReps* body_to_imu)
@@ -112,6 +152,8 @@ void ahrs_dcm_init(struct OrientationReps* body_to_imu)
   /* save body_to_imu pointer */
   ahrs_dcm.body_to_imu = body_to_imu;
 
+  ahrs_dcm.status = AHRS_DCM_UNINIT;
+
   /* Set ltp_to_imu so that body is zero */
   memcpy(&ahrs_dcm.ltp_to_imu_euler, orientationGetEulers_f(ahrs_dcm.body_to_imu),
          sizeof(struct FloatEulers));
@@ -126,6 +168,13 @@ void ahrs_dcm_init(struct OrientationReps* body_to_imu)
   ahrs_dcm.gps_course = 0;
   ahrs_dcm.gps_course_valid = FALSE;
   ahrs_dcm.gps_age = 100;
+
+  /*
+   * Subscribe to scaled IMU measurements and attach callbacks
+   */
+  AbiBindMsgIMU_GYRO_INT32(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb);
+  AbiBindMsgIMU_ACCEL_INT32(AHRS_DCM_IMU_ID, &accel_ev, accel_cb);
+  AbiBindMsgIMU_MAG_INT32(AHRS_DCM_IMU_ID, &mag_ev, mag_cb);
 }
 
 bool_t ahrs_dcm_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
@@ -149,6 +198,8 @@ bool_t ahrs_dcm_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
   RATES_COPY(bias0, *lp_gyro);
   RATES_FLOAT_OF_BFP(ahrs_dcm.gyro_bias, bias0);
 
+  ahrs_dcm.status = AHRS_DCM_RUNNING;
+
   return TRUE;
 }
 
@@ -209,7 +260,7 @@ void ahrs_dcm_update_gps(void)
 }
 
 
-void ahrs_dcm_update_accel(struct Int32Vect3* accel, float dt __attribute__((unused)))
+void ahrs_dcm_update_accel(struct Int32Vect3* accel)
 {
   ACCELS_FLOAT_OF_BFP(accel_float, *accel);
 
@@ -240,7 +291,7 @@ PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses GPS acceleration.")
 }
 
 
-void ahrs_dcm_update_mag(struct Int32Vect3* mag, float dt __attribute__((unused)))
+void ahrs_dcm_update_mag(struct Int32Vect3* mag)
 {
 #if USE_MAGNETOMETER
 #warning MAGNETOMETER FEEDBACK NOT TESTED YET

sw/airborne/subsystems/ahrs/ahrs_float_dcm.h

@@ -27,6 +27,11 @@
 #include <inttypes.h>
 #include "math/pprz_algebra_float.h"
 
+enum AhrsDCMStatus {
+  AHRS_DCM_UNINIT,
+  AHRS_DCM_RUNNING
+};
+
 struct AhrsFloatDCM {
   struct FloatRates gyro_bias;
   struct FloatRates rate_correction;
@@ -41,6 +46,8 @@ struct AhrsFloatDCM {
   uint8_t gps_age;
 
   struct OrientationReps* body_to_imu;
+
+  enum AhrsDCMStatus status;
 };
 extern struct AhrsFloatDCM ahrs_dcm;
 
@@ -77,8 +84,8 @@ extern void ahrs_dcm_init(struct OrientationReps* body_to_imu);
 extern bool_t ahrs_dcm_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
                              struct Int32Vect3* lp_mag);
 extern void ahrs_dcm_propagate(struct Int32Rates* gyro, float dt);
-extern void ahrs_dcm_update_accel(struct Int32Vect3* accel, float dt);
+extern void ahrs_dcm_update_accel(struct Int32Vect3* accel);
-extern void ahrs_dcm_update_mag(struct Int32Vect3* mag, float dt);
+extern void ahrs_dcm_update_mag(struct Int32Vect3* mag);
 extern void ahrs_dcm_update_gps(void);
 
 #endif // AHRS_FLOAT_DCM_H

sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c

@@ -42,6 +42,8 @@
 #include "math/pprz_simple_matrix.h"
 #include "generated/airframe.h"
 
+#include "subsystems/abi.h"
+
 //#include <stdio.h>
 
 #ifndef AHRS_MAG_NOISE_X
@@ -67,10 +69,49 @@ static void send_geo_mag(void) {
 }
 #endif
 
+
+/** ABI binding for IMU data.
+ * Used for gyro, accel and mag ABI messages.
+ */
+#ifndef AHRS_MLKF_IMU_ID
+#define AHRS_MLKF_IMU_ID ABI_BROADCAST
+#endif
+static abi_event gyro_ev;
+static abi_event accel_ev;
+static abi_event mag_ev;
+
+static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
+                    const struct Int32Rates* gyro)
+{
+  static uint32_t last_stamp = 0;
+  if (last_stamp > 0 && ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
+    float dt = (float)(*stamp - last_stamp) * 1e-6;
+    ahrs_mlkf_propagate((struct Int32Rates*)gyro, dt);
+  }
+  last_stamp = *stamp;
+}
+
+static void accel_cb(uint8_t sender_id __attribute__((unused)),
+                     const uint32_t* stamp __attribute__((unused)),
+                     const struct Int32Vect3* accel)
+{
+  if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
+    ahrs_mlkf_update_accel((struct Int32Vect3*)accel);
+  }
+}
+
+static void mag_cb(uint8_t sender_id __attribute__((unused)),
+                   const uint32_t* stamp __attribute__((unused)),
+                   const struct Int32Vect3* mag)
+{
+  if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
+    ahrs_mlkf_update_mag((struct Int32Vect3*)mag);
+  }
+}
+
 void ahrs_mlkf_register(void)
 {
-  ahrs_register_impl(ahrs_mlkf_init, ahrs_mlkf_align, ahrs_mlkf_propagate,
+  ahrs_register_impl(ahrs_mlkf_init, ahrs_mlkf_align, NULL);
-                     ahrs_mlkf_update_accel, ahrs_mlkf_update_mag, NULL);
 }
 
 void ahrs_mlkf_init(struct OrientationReps* body_to_imu) {
@@ -78,6 +119,8 @@ void ahrs_mlkf_init(struct OrientationReps* body_to_imu) {
   /* save body_to_imu pointer */
   ahrs_mlkf.body_to_imu = body_to_imu;
 
+  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));
@@ -102,6 +145,13 @@ void ahrs_mlkf_init(struct OrientationReps* body_to_imu) {
 
   VECT3_ASSIGN(ahrs_mlkf.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z);
 
+  /*
+   * Subscribe to scaled IMU measurements and attach callbacks
+   */
+  AbiBindMsgIMU_GYRO_INT32(AHRS_MLKF_IMU_ID, &gyro_ev, gyro_cb);
+  AbiBindMsgIMU_ACCEL_INT32(AHRS_MLKF_IMU_ID, &accel_ev, accel_cb);
+  AbiBindMsgIMU_MAG_INT32(AHRS_MLKF_IMU_ID, &mag_ev, mag_cb);
+
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
 #endif
@@ -122,6 +172,8 @@ bool_t ahrs_mlkf_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
   RATES_COPY(bias0, *lp_gyro);
   RATES_FLOAT_OF_BFP(ahrs_mlkf.gyro_bias, bias0);
 
+  ahrs_mlkf.status = AHRS_MLKF_RUNNING;
+
   return TRUE;
 }
 
@@ -131,7 +183,7 @@ void ahrs_mlkf_propagate(struct Int32Rates* gyro, float dt) {
   set_body_state_from_quat();
 }
 
-void ahrs_mlkf_update_accel(struct Int32Vect3* accel, float dt __attribute__((unused))) {
+void ahrs_mlkf_update_accel(struct Int32Vect3* accel) {
   struct FloatVect3 imu_g;
   ACCELS_FLOAT_OF_BFP(imu_g, *accel);
   const float alpha = 0.92;
@@ -145,7 +197,7 @@ void ahrs_mlkf_update_accel(struct Int32Vect3* accel, float dt __attribute__((un
 }
 
 
-void ahrs_mlkf_update_mag(struct Int32Vect3* mag, float dt __attribute__((unused))) {
+void ahrs_mlkf_update_mag(struct Int32Vect3* mag) {
   struct FloatVect3 imu_h;
   MAGS_FLOAT_OF_BFP(imu_h, *mag);
   update_state(&ahrs_mlkf.mag_h, &imu_h, &ahrs_mlkf.mag_noise);

sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h

@@ -36,6 +36,11 @@
 #include "math/pprz_orientation_conversion.h"
 #include "subsystems/ahrs.h"
 
+enum AhrsMlkfStatus {
+  AHRS_MLKF_UNINIT,
+  AHRS_MLKF_RUNNING
+};
+
 struct AhrsMlkf {
   struct FloatQuat   ltp_to_imu_quat;  ///< Rotation from LocalTangentPlane to IMU frame as unit quaternion
   struct FloatQuat   ltp_to_body_quat; ///< Rotation from LocalTangentPlane to body frame as unit quaternion
@@ -52,6 +57,8 @@ struct AhrsMlkf {
 
   /** pointer to body_to_imu rotation */
   struct OrientationReps* body_to_imu;
+
+  enum AhrsMlkfStatus status;
 };
 
 extern struct AhrsMlkf ahrs_mlkf;
@@ -63,7 +70,7 @@ extern void ahrs_mlkf_init(struct OrientationReps* body_to_imu);
 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);
-extern void ahrs_mlkf_update_accel(struct Int32Vect3* accel, float dt);
+extern void ahrs_mlkf_update_accel(struct Int32Vect3* accel);
-extern void ahrs_mlkf_update_mag(struct Int32Vect3* mag, float dt);
+extern void ahrs_mlkf_update_mag(struct Int32Vect3* mag);
 
 #endif /* AHRS_FLOAT_MLKF_H */

sw/airborne/subsystems/ahrs/ahrs_infrared.c

@@ -36,9 +36,26 @@
 #include "subsystems/gps.h"
 
 #include "state.h"
+#include "subsystems/abi.h"
 
 float heading;
 
+/** ABI binding for gyro data.
+ * Used for gyro ABI messages.
+ */
+#ifndef AHRS_INFRARED_GYRO_ID
+#define AHRS_INFRARED_GYRO_ID ABI_BROADCAST
+#endif
+static abi_event gyro_ev;
+
+static void gyro_cb(uint8_t sender_id __attribute__((unused)),
+                    const uint32_t* stamp __attribute__((unused)),
+                    const struct Int32Rates* gyro)
+{
+  stateSetBodyRates_i((struct Int32Rates*)gyro);
+}
+
+
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
 
@@ -55,40 +72,25 @@ static void send_status(void) {
 }
 #endif
 
-void ahrs_init(void) {
+void ahrs_infrared_register(void)
-  ahrs.status = AHRS_UNINIT;
+{
+  ahrs_register_impl(ahrs_infrared_init, NULL, ahrs_infrared_update_gps);
+}
+
+void ahrs_infrared_init(struct OrientationReps* body_to_imu __attribute__((unused))) {
+  ahrs.status = AHRS_RUNNING;
 
   heading = 0.;
 
+  AbiBindMsgIMU_GYRO_INT32(AHRS_INFRARED_GYRO_ID, &gyro_ev, gyro_cb);
+
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, "IR_SENSORS", send_infrared);
   register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_status);
 #endif
 }
 
-void ahrs_align(void) {
+void ahrs_infrared_update_gps(void) {
-
-  //TODO set gyro bias if used
-
-  ahrs.status = AHRS_RUNNING;
-}
-
-void ahrs_propagate(float dt __attribute__((unused))) {
-  struct FloatRates body_rate = { 0., 0., 0. };
-#ifdef ADC_CHANNEL_GYRO_P
-  body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p);
-#endif
-#ifdef ADC_CHANNEL_GYRO_Q
-  body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q);
-#endif
-#ifdef ADC_CHANNEL_GYRO_R
-  body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r);
-#endif
-  stateSetBodyRates_f(&body_rate);
-}
-
-
-void ahrs_update_gps(void) {
 
   float hspeed_mod_f = gps.gspeed / 100.;
   float course_f = gps.course / 1e7;
@@ -120,4 +122,3 @@ void ahrs_update_infrared(void) {
   stateSetNedToBodyEulers_f(&att);
 
 }
-

sw/airborne/subsystems/ahrs/ahrs_infrared.h

@@ -29,9 +29,15 @@
 #ifndef AHRS_INFRARED_H
 #define AHRS_INFRARED_H
 
-#include "subsystems/ahrs.h"
 #include "std.h"
+#include "subsystems/ahrs.h"
+#include "math/pprz_orientation_conversion.h"
 
+extern void ahrs_infrared_register(void);
+extern void ahrs_infrared_init(struct OrientationReps* body_to_imu __attribute__((unused)));
 extern void ahrs_update_infrared(void);
+extern void ahrs_infrared_update_gps(void);
+
+#define DefaultAhrsImpl ahrs_infrared
 
 #endif /* AHRS_INFRARED_H */

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c

@@ -31,6 +31,7 @@
 #include "ahrs_int_cmpl_euler.h"
 
 #include "state.h"
+#include "subsystems/abi.h"
 #include "math/pprz_trig_int.h"
 #include "math/pprz_algebra_int.h"
 
@@ -59,6 +60,50 @@ static inline void set_body_state_from_euler(void);
     while (_a < -PI_INTEG_EULER)  _a += TWO_PI_INTEG_EULER; \
   }
 
+
+/** ABI binding for IMU data.
+ * Used for gyro, accel and mag ABI messages.
+ */
+#ifndef AHRS_ICE_IMU_ID
+#define AHRS_ICE_IMU_ID ABI_BROADCAST
+#endif
+static abi_event gyro_ev;
+static abi_event accel_ev;
+static abi_event mag_ev;
+
+static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
+                    const struct Int32Rates* gyro)
+{
+  static uint32_t last_stamp = 0;
+  if (last_stamp > 0 && ahrs_ice.status == AHRS_ICE_RUNNING) {
+    float dt = (float)(*stamp - last_stamp) * 1e-6;
+    ahrs_ice_propagate((struct Int32Rates*)gyro, dt);
+  }
+  last_stamp = *stamp;
+}
+
+static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
+                     const struct Int32Vect3* accel)
+{
+  static uint32_t last_stamp = 0;
+  if (last_stamp > 0 && ahrs_ice.status == AHRS_ICE_RUNNING) {
+    float dt = (float)(*stamp - last_stamp) * 1e-6;
+    ahrs_ice_update_accel((struct Int32Vect3*)accel, dt);
+  }
+  last_stamp = *stamp;
+}
+
+static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
+                   const struct Int32Vect3* mag)
+{
+  static uint32_t last_stamp = 0;
+  if (last_stamp > 0 && ahrs_ice.status == AHRS_ICE_RUNNING) {
+    float dt = (float)(*stamp - last_stamp) * 1e-6;
+    ahrs_ice_update_mag((struct Int32Vect3*)mag, dt);
+  }
+  last_stamp = *stamp;
+}
+
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
 
@@ -100,8 +145,7 @@ static void send_bias(void) {
 
 void ahrs_ice_register(void)
 {
-  ahrs_register_impl(ahrs_ice_init, ahrs_ice_align, ahrs_ice_propagate,
+  ahrs_register_impl(ahrs_ice_init, ahrs_ice_align, NULL);
-                     ahrs_ice_update_accel, ahrs_ice_update_mag, NULL);
 }
 
 void ahrs_ice_init(struct OrientationReps* body_to_imu)
@@ -109,6 +153,8 @@ void ahrs_ice_init(struct OrientationReps* body_to_imu)
   /* save body_to_imu pointer */
   ahrs_ice.body_to_imu = body_to_imu;
 
+  ahrs_ice.status = AHRS_ICE_UNINIT;
+
   /* set ltp_to_imu so that body is zero */
   memcpy(&ahrs_ice.ltp_to_imu_euler, orientationGetEulers_i(ahrs_ice.body_to_imu),
          sizeof(struct Int32Eulers));
@@ -119,6 +165,13 @@ void ahrs_ice_init(struct OrientationReps* body_to_imu)
 
   ahrs_ice.mag_offset = AHRS_MAG_OFFSET;
 
+  /*
+   * Subscribe to scaled IMU measurements and attach callbacks
+   */
+  AbiBindMsgIMU_GYRO_INT32(AHRS_ICE_IMU_ID, &gyro_ev, gyro_cb);
+  AbiBindMsgIMU_ACCEL_INT32(AHRS_ICE_IMU_ID, &accel_ev, accel_cb);
+  AbiBindMsgIMU_MAG_INT32(AHRS_ICE_IMU_ID, &mag_ev, mag_cb);
+
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, "FILTER", send_filter);
   register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler);
@@ -144,6 +197,8 @@ bool_t ahrs_ice_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
 
   RATES_COPY(ahrs_ice.gyro_bias, *lp_gyro);
 
+  ahrs_ice.status = AHRS_ICE_RUNNING;
+
   return TRUE;
 }
 

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h

@@ -34,6 +34,11 @@
 #include "std.h"
 #include "math/pprz_algebra_int.h"
 
+enum AhrsICEStatus {
+  AHRS_ICE_UNINIT,
+  AHRS_ICE_RUNNING
+};
+
 struct AhrsIntCmplEuler {
   struct Int32Rates  gyro_bias;
   struct Int32Rates  imu_rate;
@@ -46,6 +51,8 @@ struct AhrsIntCmplEuler {
   float mag_offset;
 
   struct OrientationReps* body_to_imu;
+
+  enum AhrsICEStatus status;
 };
 
 extern struct AhrsIntCmplEuler ahrs_ice;

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c

@@ -32,7 +32,7 @@
 
 #include "subsystems/ahrs/ahrs_int_cmpl_quat.h"
 #include "subsystems/ahrs/ahrs_int_utils.h"
-
+#include "subsystems/abi.h"
 #include "state.h"
 
 #if USE_GPS
@@ -109,6 +109,51 @@ static inline void set_body_state_from_quat(void);
 static inline void UNUSED ahrs_icq_update_mag_full(struct Int32Vect3* mag, float dt);
 static inline void ahrs_icq_update_mag_2d(struct Int32Vect3* mag, float dt);
 
+/** ABI binding for IMU data.
+ * Used for gyro, accel and mag ABI messages.
+ */
+#ifndef AHRS_ICQ_IMU_ID
+#define AHRS_ICQ_IMU_ID ABI_BROADCAST
+#endif
+static abi_event gyro_ev;
+static abi_event accel_ev;
+static abi_event mag_ev;
+
+static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
+                    const struct Int32Rates* gyro)
+{
+  static uint32_t last_stamp = 0;
+  if (last_stamp > 0 && ahrs_icq.status == AHRS_ICQ_RUNNING) {
+    float dt = (float)(*stamp - last_stamp) * 1e-6;
+    ahrs_icq_propagate((struct Int32Rates*)gyro, dt);
+  }
+  last_stamp = *stamp;
+}
+
+static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
+                     const struct Int32Vect3* accel)
+{
+  static uint32_t last_stamp = 0;
+  if (last_stamp > 0 && ahrs_icq.status == AHRS_ICQ_RUNNING) {
+    float dt = (float)(*stamp - last_stamp) * 1e-6;
+    ahrs_icq_update_accel((struct Int32Vect3*)accel, dt);
+  }
+  last_stamp = *stamp;
+}
+
+static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
+                   const struct Int32Vect3* mag)
+{
+#if USE_MAGNETOMETER
+  static uint32_t last_stamp = 0;
+  if (last_stamp > 0 && ahrs_icq.status == AHRS_ICQ_RUNNING) {
+    float dt = (float)(*stamp - last_stamp) * 1e-6;
+    ahrs_icq_update_mag((struct Int32Vect3*)mag, dt);
+  }
+  last_stamp = *stamp;
+#endif
+}
+
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
 
@@ -156,8 +201,7 @@ static void send_geo_mag(void) {
 
 void ahrs_icq_register(void)
 {
-  ahrs_register_impl(ahrs_icq_init, ahrs_icq_align, ahrs_icq_propagate,
+  ahrs_register_impl(ahrs_icq_init, ahrs_icq_align, ahrs_icq_update_gps);
-                     ahrs_icq_update_accel, ahrs_icq_update_mag, ahrs_icq_update_gps);
 }
 
 
@@ -166,6 +210,8 @@ void ahrs_icq_init(struct OrientationReps* body_to_imu) {
   /* save body_to_imu pointer */
   ahrs_icq.body_to_imu = body_to_imu;
 
+  ahrs_icq.status = AHRS_ICQ_UNINIT;
+
   ahrs_icq.ltp_vel_norm_valid = FALSE;
   ahrs_icq.heading_aligned = FALSE;
 
@@ -201,6 +247,13 @@ void ahrs_icq_init(struct OrientationReps* body_to_imu) {
   ahrs_icq.accel_cnt = 0;
   ahrs_icq.mag_cnt = 0;
 
+  /*
+   * Subscribe to scaled IMU measurements and attach callbacks
+   */
+  AbiBindMsgIMU_GYRO_INT32(AHRS_ICQ_IMU_ID, &gyro_ev, gyro_cb);
+  AbiBindMsgIMU_ACCEL_INT32(AHRS_ICQ_IMU_ID, &accel_ev, accel_cb);
+  AbiBindMsgIMU_MAG_INT32(AHRS_ICQ_IMU_ID, &mag_ev, mag_cb);
+
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat);
   register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler);
@@ -235,6 +288,8 @@ bool_t ahrs_icq_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
   RATES_COPY(ahrs_icq.high_rez_bias, *lp_gyro);
   INT_RATES_LSHIFT(ahrs_icq.high_rez_bias, ahrs_icq.high_rez_bias, 28);
 
+  ahrs_icq.status = AHRS_ICQ_RUNNING;
+
   return TRUE;
 }
 

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h

@@ -35,6 +35,11 @@
 #include "std.h"
 #include "math/pprz_algebra_int.h"
 
+enum AhrsICQStatus {
+  AHRS_ICQ_UNINIT,
+  AHRS_ICQ_RUNNING
+};
+
 /**
  * Ahrs implementation specifc values
  */
@@ -92,6 +97,8 @@ struct AhrsIntCmplQuat {
   uint16_t mag_cnt;   ///< number of propagations since last mag update
 
   struct OrientationReps* body_to_imu;
+
+  enum AhrsICQStatus status; ///< status of the AHRS, AHRS_ICQ_UNINIT or AHRS_ICQ_RUNNING
 };
 
 extern struct AhrsIntCmplQuat ahrs_icq;

sw/airborne/subsystems/ahrs/ahrs_sim.c

@@ -30,6 +30,7 @@
 #include "subsystems/ahrs/ahrs_sim.h"
 #include "math/pprz_algebra_float.h"
 #include "generated/airframe.h"
+#include "state.h"
 
 extern float sim_phi;
 extern float sim_theta;
@@ -37,7 +38,6 @@ extern float sim_psi;
 extern float sim_p;
 extern float sim_q;
 extern float sim_r;
-extern bool_t ahrs_sim_available;
 
 // for sim of e.g. Xsens ins
 #ifndef INS_ROLL_NEUTRAL_DEFAULT
@@ -61,31 +61,8 @@ void update_ahrs_from_sim(void) {
 }
 
 
-void ahrs_init(void) {
+void ahrs_sim_init(void) {
   //ahrs_float.status = AHRS_UNINIT;
   // set to running for now
   ahrs.status = AHRS_RUNNING;
-
-  ahrs_sim_available = FALSE;
-
 }
-
-void ahrs_align(void)
-{
-  /* Currently not really simulated
-   * body and imu have the same frame and always set to true value from sim
-   */
-
-  update_ahrs_from_sim();
-
-  ahrs.status = AHRS_RUNNING;
-}
-
-
-void ahrs_propagate(float dt __attribute__((unused))) {
-  if (ahrs_sim_available) {
-    update_ahrs_from_sim();
-    ahrs_sim_available = FALSE;
-  }
-}
-

sw/airborne/subsystems/ahrs/ahrs_sim.h

@@ -32,11 +32,10 @@
 #include "subsystems/ahrs.h"
 #include "std.h"
 
-extern bool_t ahrs_sim_available;
-
 extern float ins_roll_neutral;
 extern float ins_pitch_neutral;
 
 extern void update_ahrs_from_sim(void);
+extern void ahrs_sim_init(void);
 
 #endif /* AHRS_SIM_H */

sw/airborne/subsystems/ins/ins_ardrone2.c

@@ -92,8 +92,8 @@ void ins_reset_altitude_ref( void ) {
 
 void ins_propagate(float __attribute__((unused)) dt) {
   /* untilt accels and speeds */
-  float_rmat_transp_vmult(&ins_impl.ltp_accel, stateGetNedToBodyRMat_f(), &ahrs_impl.accel);
+  float_rmat_transp_vmult(&ins_impl.ltp_accel, stateGetNedToBodyRMat_f(), &ahrs_ardrone2.accel);
-  float_rmat_transp_vmult(&ins_impl.ltp_speed, stateGetNedToBodyRMat_f(), &ahrs_impl.speed);
+  float_rmat_transp_vmult(&ins_impl.ltp_speed, stateGetNedToBodyRMat_f(), &ahrs_ardrone2.speed);
 
   //Add g to the accelerations
   ins_impl.ltp_accel.z += 9.81;
@@ -105,7 +105,7 @@ void ins_propagate(float __attribute__((unused)) dt) {
   //Don't set the height if we use the one from the gps
 #if !USE_GPS_HEIGHT
   //Set the height and save the position
-  ins_impl.ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
+  ins_impl.ltp_pos.z = -(ahrs_ardrone2.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
   stateSetPositionNed_i(&ins_impl.ltp_pos);
 #endif
 }