Merge pull request #856 from paparazzi/ahrs_refactor

AHRS API/Interface refactor - each AHRS implementation has it's own function names, to basically make it possible to run multiple at the same time - input data passed as pointer arguments - wrappers that subscribe to ABI messages (gyro, accel, mag) which call the actual implementation - makes it easier to run the actual algorithm e.g. for testing without ABI - body_to_imu quaternion is also set via ABI - IMU measurements are published via ABI from the gyro/accel/mag callbacks from main for now - each ahrs implementation has an `is_aligned` bool for now... - MLKF: function to only update heading from mag (default now, define AHRS_MAG_UPDATE_ALL_AXES to update all axes as previously) Merging now as it is already useful. Still TODO: - publish GPS via ABI as well (and get rid of global ahrs_update_gps) - update all IMU implementations to publish ABI messages directly - pimp or get rid of ahrs_register_impl (to actually handle multiple AHRS at the same time)
2026-06-03 21:33:39 +08:00 · 2015-03-01 22:07:23 +01:00
parent e7f2c979d0 bb5ba6e716
commit 2dd069e80a
72 changed files with 2783 additions and 1697 deletions
@@ -20,6 +20,32 @@
      <field name="temp" type="float" unit="deg Celcius"/>
    </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>
    <message name="IMU_LOWPASSED" id="7">
      <field name="stamp" type="uint32_t" unit="us"/>
      <field name="gyro" type="struct Int32Rates *"/>
      <field name="accel" type="struct Int32Vect3 *"/>
      <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>
@@ -33,7 +33,12 @@
    <subsystem name="imu"           type="lisa_mx_v2.1"/>
    <subsystem name="gps"           type="ublox"/>
    <subsystem name="stabilization" type="int_quat"/>
-    <subsystem name="ahrs"          type="float_mlkf"/>
+    <subsystem name="ahrs"          type="float_mlkf">
 	    <!-- Uncomment to enable all axis update from the Magnetometer. Do
 		 it only if you have a very well calibrated and tested
 		 magnetometer otherwise your attitude will drift. -->
 	    <!--define name="AHRS_MAG_UPDATE_ALL_AXES" value="1"/-->
    </subsystem>
    <subsystem name="ins" type="hff"/>
    <!--define name="KILL_ON_GROUND_DETECT" value="TRUE"/-->
@@ -21,10 +21,11 @@ ifneq ($(AHRS_ALIGNER_LED),none)
  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
-AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\"
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
 AHRS_CFLAGS += -DAHRS_PROPAGATE_QUAT
 AHRS_SRCS   += subsystems/ahrs.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_float_cmpl.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_float_cmpl_wrapper.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
 ifdef AHRS_PROPAGATE_FREQUENCY
@@ -21,10 +21,11 @@ ifneq ($(AHRS_ALIGNER_LED),none)
  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
-AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\"
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
 AHRS_CFLAGS += -DAHRS_PROPAGATE_RMAT
 AHRS_SRCS   += subsystems/ahrs.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_float_cmpl.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_float_cmpl_wrapper.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
 ifdef AHRS_PROPAGATE_FREQUENCY
@@ -5,7 +5,7 @@
 USE_MAGNETOMETER ?= 0
 AHRS_ALIGNER_LED ?= none
-AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm_wrapper.h\"
 AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
 AHRS_CFLAGS += -DUSE_AHRS
@@ -16,6 +16,7 @@ endif
 AHRS_SRCS   += $(SRC_SUBSYSTEMS)/ahrs.c
 AHRS_SRCS   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
 AHRS_SRCS   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
 AHRS_SRCS   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm_wrapper.c
 ifneq ($(AHRS_ALIGNER_LED),none)
@@ -4,17 +4,8 @@
 GX3_PORT ?= UART3
 GX3_BAUD ?= B921600
 AHRS_ALIGNER_LED ?= none
 AHRS_CFLAGS  = -DUSE_AHRS
 AHRS_CFLAGS += -DUSE_IMU
 AHRS_CFLAGS += -DUSE_IMU_FLOAT
 AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
 ifneq ($(AHRS_ALIGNER_LED),none)
  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
 AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_gx3.h\"
 AHRS_SRCS   += $(SRC_SUBSYSTEMS)/ahrs.c
@@ -22,9 +22,10 @@ ifneq ($(AHRS_ALIGNER_LED),none)
  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
-AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat.h\"
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
 AHRS_SRCS   += subsystems/ahrs.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_int_cmpl_quat.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
@@ -19,10 +19,11 @@ ifneq ($(AHRS_ALIGNER_LED),none)
  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
-AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\"
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
 AHRS_CFLAGS += -DAHRS_PROPAGATE_QUAT
 AHRS_SRCS   += subsystems/ahrs.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_float_cmpl.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_float_cmpl_wrapper.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
 ifdef AHRS_PROPAGATE_FREQUENCY
@@ -18,10 +18,11 @@ ifneq ($(AHRS_ALIGNER_LED),none)
  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
-AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\"
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
 AHRS_CFLAGS += -DAHRS_PROPAGATE_RMAT
 AHRS_SRCS   += subsystems/ahrs.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_float_cmpl.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_float_cmpl_wrapper.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
 ifdef AHRS_PROPAGATE_FREQUENCY
@@ -18,9 +18,10 @@ ifneq ($(AHRS_ALIGNER_LED),none)
  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
-AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf.h\"
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
 AHRS_SRCS   += subsystems/ahrs.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_float_mlkf.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_float_mlkf_wrapper.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
 ifdef AHRS_PROPAGATE_FREQUENCY
@@ -1,18 +1,11 @@
-# Rotorcraft AHRS module for GX3
+# AHRS subsystem for GX3
 # 2013, Utah State University, http://aggieair.usu.edu/
 GX3_PORT ?= UART3
 GX3_BAUD ?= B921600
 AHRS_ALIGNER_LED ?= none
 AHRS_CFLAGS  = -DUSE_AHRS
 AHRS_CFLAGS += -DAHRS_FLOAT
 AHRS_CFLAGS += -DUSE_IMU
 AHRS_CFLAGS += -DUSE_IMU_FLOAT
 ifneq ($(AHRS_ALIGNER_LED),none)
  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
 AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_gx3.h\"
 AHRS_SRCS   += $(SRC_SUBSYSTEMS)/ahrs.c
@@ -17,9 +17,10 @@ ifneq ($(AHRS_ALIGNER_LED),none)
  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
-AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler.h\"
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\"
 AHRS_SRCS   += subsystems/ahrs.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_int_cmpl_euler.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
 ap.CFLAGS += $(AHRS_CFLAGS)
@@ -19,9 +19,10 @@ ifneq ($(AHRS_ALIGNER_LED),none)
  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
-AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat.h\"
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
 AHRS_SRCS   += subsystems/ahrs.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_int_cmpl_quat.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
 AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
 ifdef AHRS_PROPAGATE_FREQUENCY
@@ -19,7 +19,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>
@@ -23,7 +23,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>
@@ -4,11 +4,11 @@
  <dl_settings>
    <dl_settings NAME="AHRS">
-       <dl_setting var="ahrs_impl.gravity_heuristic_factor" min="0" step="1" max="50" module="subsystems/ahrs/ahrs_float_cmpl" shortname="g_heuristic" param="AHRS_GRAVITY_HEURISTIC_FACTOR"/>
+       <dl_setting var="ahrs_fc.gravity_heuristic_factor" min="0" step="1" max="50" module="subsystems/ahrs/ahrs_float_cmpl" shortname="g_heuristic" param="AHRS_GRAVITY_HEURISTIC_FACTOR"/>
-       <dl_setting var="ahrs_impl.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_float_cmpl" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s"/>
+       <dl_setting var="ahrs_fc.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_float_cmpl" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s"/>
-       <dl_setting var="ahrs_impl.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_float_cmpl" shortname="acc_zeta" param="AHRS_ACCEL_ZETA"/>
+       <dl_setting var="ahrs_fc.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_float_cmpl" shortname="acc_zeta" param="AHRS_ACCEL_ZETA"/>
-       <dl_setting var="ahrs_impl.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_float_cmpl" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s"/>
+       <dl_setting var="ahrs_fc.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_float_cmpl" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s"/>
-       <dl_setting var="ahrs_impl.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_float_cmpl" shortname="mag_zeta" param="AHRS_MAG_ZETA"/>
+       <dl_setting var="ahrs_fc.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_float_cmpl" shortname="mag_zeta" param="AHRS_MAG_ZETA"/>
    </dl_settings>
  </dl_settings>
@@ -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>
@@ -4,7 +4,7 @@
  <dl_settings>
    <dl_settings NAME="Filter">
-       <dl_setting var="ahrs_impl.reinj_1" min="512" step="512" max="262144" module="subsystems/ahrs/ahrs_int_cmpl_euler" shortname="reinj_1"/>
+       <dl_setting var="ahrs_ice.reinj_1" min="512" step="512" max="262144" module="subsystems/ahrs/ahrs_int_cmpl_euler" shortname="reinj_1"/>
    </dl_settings>
  </dl_settings>
@@ -4,11 +4,11 @@
  <dl_settings>
    <dl_settings NAME="AHRS">
-       <dl_setting var="ahrs_impl.gravity_heuristic_factor" min="0" step="1" max="50" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="g_heuristic" param="AHRS_GRAVITY_HEURISTIC_FACTOR"/>
+       <dl_setting var="ahrs_icq.gravity_heuristic_factor" min="0" step="1" max="50" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="g_heuristic" param="AHRS_GRAVITY_HEURISTIC_FACTOR"/>
-       <dl_setting var="ahrs_impl.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s" handler="SetAccelOmega"/>
+       <dl_setting var="ahrs_icq.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s" handler="SetAccelOmega"/>
-       <dl_setting var="ahrs_impl.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_zeta" param="AHRS_ACCEL_ZETA" handler="SetAccelZeta"/>
+       <dl_setting var="ahrs_icq.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_zeta" param="AHRS_ACCEL_ZETA" handler="SetAccelZeta"/>
-       <dl_setting var="ahrs_impl.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s" handler="SetMagOmega"/>
+       <dl_setting var="ahrs_icq.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s" handler="SetMagOmega"/>
-       <dl_setting var="ahrs_impl.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_zeta" param="AHRS_MAG_ZETA" handler="SetMagZeta"/>
+       <dl_setting var="ahrs_icq.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_zeta" param="AHRS_MAG_ZETA" handler="SetMagZeta"/>
    </dl_settings>
  </dl_settings>
@@ -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
@@ -25,8 +24,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;
 }
@@ -36,8 +33,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;
 }
@@ -43,28 +43,28 @@ 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
+  if(thrust > 0 && (ahrs_ardrone2.control_state == CTRL_DEFAULT || ahrs_ardrone2.control_state == CTRL_INIT
-                     || ahrs_impl.control_state == CTRL_LANDED)) {
+                    || 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
+  } else if(thrust < -0.9 && !(ahrs_ardrone2.control_state == CTRL_DEFAULT ||
-                                || ahrs_impl.control_state == CTRL_LANDED)) {
+            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
+  if ((ahrs_ardrone2.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 &&
-      || ahrs_impl.control_state == CTRL_HOVERING)) {
+      (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
+  if ((ahrs_ardrone2.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 &&
-      || ahrs_impl.control_state == CTRL_HOVERING)) {
+      (ahrs_ardrone2.control_state == CTRL_FLYING || ahrs_ardrone2.control_state == CTRL_HOVERING)) {
    at_com_send_pcmd(1, thrust, roll, pitch, yaw);
  }
@@ -157,7 +157,7 @@ static void send_navdata(struct transport_tx *trans, struct link_device *dev)
 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
 {
  uint8_t mde = 3;
-  if (ahrs.status == AHRS_UNINIT) { mde = 2; }
+  if (!DefaultAhrsImpl.is_aligned) { mde = 2; }
  if (imu_lost) { mde = 5; }
  uint16_t val = imu_lost_counter;
  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val);
@@ -126,6 +126,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
@@ -144,7 +149,7 @@ volatile uint8_t ahrs_timeout_counter = 0;
 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
 {
  uint8_t mde = 3;
-  if (ahrs.status == AHRS_UNINIT) { mde = 2; }
+  if (!DefaultAhrsImpl.is_aligned) { mde = 2; }
  if (ahrs_timeout_counter > 10) { mde = 5; }
  uint16_t val = 0;
  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val);
@@ -195,8 +200,15 @@ void init_ap(void)
 #endif
 #if USE_AHRS
 #if defined SITL && !USE_NPS && !USE_INFRARED
  ahrs_sim_init();
 #else
  ahrs_init();
  DefaultAhrsRegister();
 #endif
 #endif
  ins_init();
 #if USE_AHRS && USE_IMU
  register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
@@ -206,8 +218,6 @@ void init_ap(void)
  baro_init();
 #endif
  ins_init();
  /************* Links initialization ***************/
 #if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
  link_mcu_init();
@@ -261,6 +271,11 @@ void init_ap(void)
  ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
  ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
  ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
 #if USE_IMU
  // send body_to_imu from here for now
  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
 #endif
 }
@@ -615,9 +630,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(1. / PERIODIC_FREQUENCY);
 #endif
 #if USE_GPS
@@ -738,10 +752,10 @@ void event_task_ap(void)
 #if USE_GPS
 static inline void on_gps_solution(void)
 {
  ins_update_gps();
 #if USE_AHRS
  ahrs_update_gps();
 #endif
  ins_update_gps();
 #ifdef GPS_TRIGGERED_FUNCTION
  GPS_TRIGGERED_FUNCTION();
 #endif
@@ -749,69 +763,53 @@ static inline void on_gps_solution(void)
 #endif
 #if USE_AHRS
 #if USE_IMU
 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_UNINIT) {
+
-    ahrs_update_accel(dt);
+  AbiSendMsgIMU_ACCEL_INT32(1, now_ts, &imu.accel);
  }
 }
 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();
  // 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_VAR(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_aligner.status != AHRS_ALIGNER_LOCKED) {
  if (ahrs.status == AHRS_UNINIT) {
    ahrs_aligner_run();
    if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
      ahrs_align();
    }
    return;
  }
 #endif
  ahrs_propagate(dt);
 #if defined SITL && USE_NPS
  if (nps_bypass_ahrs) { sim_overwrite_ahrs(); }
 #endif
 #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 INS propagation.")
 PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
 #endif
  ins_propagate(dt);
 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
  new_ins_attitude = 1;
 #endif
@@ -821,28 +819,12 @@ static inline void on_gyro_event(void)
 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(dt);
  }
 #endif
 }
 #endif // USE_IMU
 #endif // USE_AHRS
@@ -92,7 +92,7 @@ bool_t   autopilot_detect_ground_once;
 #include "subsystems/ahrs.h"
 static inline int ahrs_is_aligned(void)
 {
-  return (ahrs.status == AHRS_RUNNING);
+  return DefaultAhrsImpl.is_aligned;
 }
 #else
 PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")
@@ -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"
@@ -112,6 +114,10 @@ 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_gps_event(void);
@@ -160,10 +166,14 @@ STATIC_INLINE void main_init(void)
  baro_init();
 #endif
  imu_init();
 #if USE_AHRS_ALIGNER
  ahrs_aligner_init();
 #endif
  ahrs_init();
  ins_init();
  DefaultAhrsRegister();
 #if USE_GPS
  gps_init();
 #endif
@@ -190,6 +200,9 @@ STATIC_INLINE void main_init(void)
 #if USE_BARO_BOARD
  baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
 #endif
  // send body_to_imu from here for now
  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
 }
 STATIC_INLINE void handle_periodic_tasks(void)
@@ -343,61 +356,48 @@ STATIC_INLINE void main_event(void)
 }
-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
  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_UNINIT) {
+  AbiSendMsgIMU_ACCEL_INT32(1, now_ts, &imu.accel);
    ahrs_update_accel(dt);
  }
 }
-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();
  imu_scale_gyro(&imu);
  AbiSendMsgIMU_GYRO_INT32(1, now_ts, &imu.gyro_prev);
 #if USE_AHRS_ALIGNER
  if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) {
    ahrs_aligner_run();
    return;
  }
 #endif
 #ifdef SITL
  if (nps_bypass_ahrs) sim_overwrite_ahrs();
 #endif
 #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);
 #endif
  ins_propagate(dt);
  imu_scale_gyro(&imu);
  if (ahrs.status == AHRS_UNINIT) {
    ahrs_aligner_run();
    if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
      ahrs_align();
    }
  } else {
    ahrs_propagate(dt);
 #ifdef SITL
    if (nps_bypass_ahrs) { sim_overwrite_ahrs(); }
 #endif
    ins_propagate(dt);
  }
 #ifdef USE_VEHICLE_INTERFACE
  vi_notify_imu_available();
 #endif
@@ -417,27 +417,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(dt);
  }
 #endif // USE_MAGNETOMETER
 #ifdef USE_VEHICLE_INTERFACE
  vi_notify_mag_available();
@@ -296,7 +296,7 @@ static inline void mavlink_send_heartbeat(void)
 #else
  uint8_t mav_type = MAV_TYPE_QUADROTOR;
 #endif
-  if (ahrs.status == AHRS_RUNNING) {
+  if (DefaultAhrsImpl.is_aligned) {
    if (kill_throttle) {
      mav_state = MAV_STATE_STANDBY;
    }
@@ -76,11 +76,11 @@ void geo_mag_event(void)
    // copy to ahrs
 #ifdef AHRS_FLOAT
-    VECT3_COPY(ahrs_impl.mag_h, geo_mag.vect);
+    VECT3_COPY(DefaultAhrsImpl.mag_h, geo_mag.vect);
 #else
    // convert to MAG_BFP and copy to ahrs
-    VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x), MAG_BFP_OF_REAL(geo_mag.vect.y),
+    VECT3_ASSIGN(DefaultAhrsImpl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x),
-                 MAG_BFP_OF_REAL(geo_mag.vect.z));
+                 MAG_BFP_OF_REAL(geo_mag.vect.y), MAG_BFP_OF_REAL(geo_mag.vect.z));
 #endif
    geo_mag.ready = TRUE;
@@ -0,0 +1,43 @@
 /*
 * 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"
 struct AhrsChimu {
  bool_t is_aligned;
 };
 extern struct AhrsChimu ahrs_chimu;
 #define DefaultAhrsImpl ahrs_chimu
 extern void ahrs_chimu_register(void);
 extern void ahrs_chimu_init(void);
 #endif
@@ -26,6 +26,7 @@
 #include "ins_module.h"
 #include "imu_chimu.h"
 #include "ahrs_chimu.h"
 #include "led.h"
@@ -34,9 +35,18 @@ CHIMU_PARSER_DATA CHIMU_DATA;
 INS_FORMAT ins_roll_neutral;
 INS_FORMAT ins_pitch_neutral;
-void ahrs_init(void)
+struct AhrsChimu ahrs_chimu;
 void ahrs_chimu_update_gps(void);
 void ahrs_chimu_register(void)
 {
-  ahrs.status = AHRS_UNINIT;
+  ahrs_register_impl(ahrs_chimu_init, ahrs_chimu_update_gps);
 }
 void ahrs_chimu_init(void)
 {
  ahrs_chimu.is_aligned = FALSE;
  // uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
  uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 };  // 50Hz attitude only + SPI
@@ -68,10 +78,6 @@ void ahrs_init(void)
  InsSend(rate, 12);
 }
 void ahrs_align(void)
 {
  ahrs.status = AHRS_RUNNING;
 }
 void parse_ins_msg(void)
 {
@@ -98,6 +104,8 @@ void parse_ins_msg(void)
          0.
        }; // FIXME rate r
        stateSetBodyRates_f(&rates);
        //FIXME
        ahrs_chimu.is_aligned = TRUE;
      } else if (CHIMU_DATA.m_MsgID == 0x02) {
 #if CHIMU_DOWNLINK_IMMEDIATE
        RunOnceEvery(25, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0],
@@ -109,7 +117,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,11 +138,3 @@ void ahrs_update_gps(void)
  // Downlink Send
 }
 void ahrs_propagate(float dt __attribute__((unused)))
 {
 }
 void ahrs_update_accel(float dt __attribute__((unused)))
 {
 }
@@ -30,9 +30,16 @@ CHIMU_PARSER_DATA CHIMU_DATA;
 INS_FORMAT ins_roll_neutral;
 INS_FORMAT ins_pitch_neutral;
-void ahrs_init(void)
+struct AhrsChimu ahrs_chimu;
 void ahrs_chimu_register(void)
 {
-  ahrs.status = AHRS_UNINIT;
+  ahrs_register_impl(ahrs_chimu_init, NULL);
 }
 void ahrs_chimu_init(void)
 {
  ahrs_chimu.is_aligned = FALSE;
  uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
  uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 };  // 50Hz attitude only + SPI
@@ -61,10 +68,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 +89,7 @@ void parse_ins_msg(void)
          CHIMU_DATA.m_attitude.euler.psi
        };
        stateSetNedToBodyEulers_f(&att);
        ahrs_chimu.is_aligned = TRUE;
 #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);
@@ -95,7 +99,3 @@ void parse_ins_msg(void)
    }
  }
 }
 void ahrs_update_gps(void)
 {
 }
@@ -32,7 +32,7 @@
 #if MODULE_HMC58XX_UPDATE_AHRS
 #include "subsystems/imu.h"
-#include "subsystems/ahrs.h"
+#include "subsystems/abi.h"
 #ifndef HMC58XX_CHAN_X
 #define HMC58XX_CHAN_X 0
@@ -60,20 +60,13 @@ void mag_hmc58xx_module_periodic(void)
 void mag_hmc58xx_module_event(void)
 {
 #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;
 #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
  hmc58xx_event(&mag_hmc58xx);
 #if MODULE_HMC58XX_UPDATE_AHRS
  if (mag_hmc58xx.data_available) {
 #if MODULE_HMC58XX_UPDATE_AHRS
    // current timestamp
    uint32_t now_ts = get_sys_time_usec();
    // set channel order
    struct Int32Vect3 mag = {
      (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
@@ -84,26 +77,16 @@ void mag_hmc58xx_module_event(void)
    VECT3_COPY(imu.mag_unscaled, mag);
    // scale vector
    imu_scale_mag(&imu);
-    // update ahrs
+
-    if (ahrs.status == AHRS_RUNNING) {
+    AbiSendMsgIMU_MAG_INT32(MAG_HMC58XX_SENDER_ID, now_ts, &imu.mag);
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
      // 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;
 #endif
      ahrs_update_mag(dt);
    }
    mag_hmc58xx.data_available = FALSE;
  }
 #endif
 #if MODULE_HMC58XX_SYNC_SEND
  if (mag_hmc58xx.data_available) {
    mag_hmc58xx_report();
    mag_hmc58xx.data_available = FALSE;
  }
 #endif
 #if MODULE_HMC58XX_UPDATE_AHRS ||  MODULE_HMC58XX_SYNC_SEND
    mag_hmc58xx.data_available = FALSE;
 #endif
  }
 }
 void mag_hmc58xx_report(void)
@@ -103,5 +103,12 @@
 #define AGL_SONAR_NPS_ID 3
 #endif
 /*
 * IDs of magnetometer sensors (including IMUs with mag)
 */
 #ifndef MAG_HMC58XX_SENDER_ID
 #define MAG_HMC58XX_SENDER_ID 2
 #endif
 #endif /* ABI_SENDER_IDS_H */
@@ -29,12 +29,23 @@
 struct Ahrs ahrs;
-// weak functions, used if not explicitly provided by implementation
+void ahrs_register_impl(AhrsInit init, AhrsUpdateGps update_gps)
 {
  ahrs.init = init;
  ahrs.update_gps = update_gps;
-void WEAK ahrs_propagate(float dt __attribute__((unused))) {}
+  ahrs.init();
 }
-void WEAK ahrs_update_accel(float dt __attribute__((unused))) {}
+void ahrs_init(void)
 {
  ahrs.init = NULL;
  ahrs.update_gps = NULL;
 }
-void WEAK ahrs_update_mag(float dt __attribute__((unused))) {}
+void ahrs_update_gps(void)
-
+{
-void WEAK ahrs_update_gps(void) {}
+  if (ahrs.update_gps != NULL) {
    ahrs.update_gps();
  }
 }
@@ -28,61 +28,35 @@
 #define AHRS_H
 #include "std.h"
 #include "math/pprz_algebra_int.h"
 #include "math/pprz_algebra_float.h"
 #include "state.h"
 #define AHRS_UNINIT  0
 #define AHRS_RUNNING 1
 /* underlying includes (needed for parameters) */
 #ifdef AHRS_TYPE_H
 #include AHRS_TYPE_H
 #endif
 typedef void (*AhrsInit)(void);
 typedef void (*AhrsUpdateGps)(void);
 /** Attitude and Heading Reference System state */
 struct Ahrs {
-  uint8_t status; ///< status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
+  /* function pointers to actual implementation, set by ahrs_register_impl */
  AhrsInit init;
  AhrsUpdateGps update_gps;
 };
 /** global AHRS state */
 extern struct Ahrs ahrs;
 extern void ahrs_register_impl(AhrsInit init, AhrsUpdateGps update_gps);
 /** AHRS initialization. Called at startup.
- *  Needs to be implemented by each AHRS algorithm.
+ * Initialized the global AHRS struct.
 */
 extern void ahrs_init(void);
 /** Aligns the AHRS. Called after ahrs_aligner has run to set initial attitude and biases.
 *  Must set the ahrs status to AHRS_RUNNING.
 *  Needs to be implemented by each AHRS algorithm.
 */
 extern void ahrs_align(void);
 /** Propagation. Usually integrates the gyro rates to angles.
 *  Reads the global #imu data struct.
 *  Does nothing if not implemented by specific AHRS algorithm.
 *  @param dt time difference since last propagation in seconds
 */
 extern void ahrs_propagate(float dt);
 /** Update AHRS state with accerleration measurements.
 *  Reads the global #imu data struct.
 *  Does nothing if not implemented by specific AHRS algorithm.
 *  @param dt time difference since last update in seconds
 */
 extern void ahrs_update_accel(float dt);
 /** Update AHRS state with magnetometer measurements.
 *  Reads the global #imu data struct.
 *  Does nothing if not implemented by specific AHRS algorithm.
 *  @param dt time difference since last update in seconds
 */
 extern void ahrs_update_mag(float dt);
 /** Update AHRS state with GPS measurements.
 *  Calls implementation if registered.
 *  Reads the global #gps data struct.
 *  Does nothing if not implemented by specific AHRS algorithm.
 */
 extern void ahrs_update_gps(void);
@@ -31,6 +31,8 @@
 #include <stdlib.h> /* for abs() */
 #include "subsystems/imu.h"
 #include "led.h"
 #include "subsystems/abi.h"
 #include "mcu_periph/sys_time.h"
 struct AhrsAligner ahrs_aligner;
@@ -45,21 +47,23 @@ static uint32_t samples_idx;
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
-static void send_aligner(struct transport_tx *trans, struct link_device *dev) {
+static void send_aligner(struct transport_tx *trans, struct link_device *dev)
 {
  pprz_msg_send_FILTER_ALIGNER(trans, dev, AC_ID,
-      &ahrs_aligner.lp_gyro.p,
+                               &ahrs_aligner.lp_gyro.p,
-      &ahrs_aligner.lp_gyro.q,
+                               &ahrs_aligner.lp_gyro.q,
-      &ahrs_aligner.lp_gyro.r,
+                               &ahrs_aligner.lp_gyro.r,
-      &imu.gyro.p,
+                               &imu.gyro.p,
-      &imu.gyro.q,
+                               &imu.gyro.q,
-      &imu.gyro.r,
+                               &imu.gyro.r,
-      &ahrs_aligner.noise,
+                               &ahrs_aligner.noise,
-      &ahrs_aligner.low_noise_cnt,
+                               &ahrs_aligner.low_noise_cnt,
-      &ahrs_aligner.status);
+                               &ahrs_aligner.status);
 }
 #endif
-void ahrs_aligner_init(void) {
+void ahrs_aligner_init(void)
 {
  ahrs_aligner.status = AHRS_ALIGNER_RUNNING;
  INT_RATES_ZERO(gyro_sum);
@@ -81,7 +85,8 @@ void ahrs_aligner_init(void) {
 #define LOW_NOISE_TIME          5
 #endif
-void ahrs_aligner_run(void) {
+void ahrs_aligner_run(void)
 {
  RATES_ADD(gyro_sum,  imu.gyro);
  VECT3_ADD(accel_sum, imu.accel);
@@ -96,15 +101,16 @@ void ahrs_aligner_run(void) {
  if (samples_idx >= SAMPLES_NB) {
    int32_t avg_ref_sensor = accel_sum.z;
-    if ( avg_ref_sensor >= 0)
+    if (avg_ref_sensor >= 0) {
      avg_ref_sensor += SAMPLES_NB / 2;
-    else
+    } else {
      avg_ref_sensor -= SAMPLES_NB / 2;
    }
    avg_ref_sensor /= SAMPLES_NB;
    ahrs_aligner.noise = 0;
    int i;
-    for (i=0; i<SAMPLES_NB; i++) {
+    for (i = 0; i < SAMPLES_NB; i++) {
      int32_t diff = ref_sensor_samples[i] - avg_ref_sensor;
      ahrs_aligner.noise += abs(diff);
    }
@@ -118,17 +124,20 @@ void ahrs_aligner_run(void) {
    INT_VECT3_ZERO(mag_sum);
    samples_idx = 0;
-    if (ahrs_aligner.noise < LOW_NOISE_THRESHOLD)
+    if (ahrs_aligner.noise < LOW_NOISE_THRESHOLD) {
      ahrs_aligner.low_noise_cnt++;
-    else
+    } else if (ahrs_aligner.low_noise_cnt > 0) {
-      if ( ahrs_aligner.low_noise_cnt > 0)
+      ahrs_aligner.low_noise_cnt--;
-        ahrs_aligner.low_noise_cnt--;
+    }
    if (ahrs_aligner.low_noise_cnt > LOW_NOISE_TIME) {
      ahrs_aligner.status = AHRS_ALIGNER_LOCKED;
 #ifdef AHRS_ALIGNER_LED
      LED_ON(AHRS_ALIGNER_LED);
 #endif
      uint32_t now_ts = get_sys_time_usec();
      AbiSendMsgIMU_LOWPASSED(ABI_BROADCAST, now_ts, &ahrs_aligner.lp_gyro,
                              &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
    }
  }
@@ -32,6 +32,7 @@
 # include <stdio.h>
 #endif
 #include "subsystems/ahrs.h"
 #include "ahrs_ardrone2.h"
 #include "state.h"
 #include "math/pprz_algebra_float.h"
@@ -42,79 +43,87 @@
 #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
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
-static void send_ahrs_ad2(struct transport_tx *trans, struct link_device *dev) {
+static void send_ahrs_ad2(struct transport_tx *trans, struct link_device *dev)
 {
  pprz_msg_send_AHRS_ARDRONE2(trans, dev, AC_ID,
-      &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);
 }
 void ahrs_ardrone2_init(void)
 {
  init_at_com();
  //Set navdata_demo to FALSE and flat trim the ar drone
  at_com_send_config("general:navdata_demo", "FALSE");
  at_com_send_ftrim();
-  ahrs.status = AHRS_RUNNING;
+  ahrs_ardrone2.is_aligned = TRUE;
 #if PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, "AHRS_ARDRONE2", send_ahrs_ad2);
 #endif
 }
 void ahrs_align(void) {
 }
 #ifdef ARDRONE2_DEBUG
-static void dump(const void *_b, size_t s) {
+static void dump(const void *_b, size_t s)
 {
  const unsigned char *b = _b;
  size_t n;
-  for(n = 0; n < s; ++n) {
+  for (n = 0; n < s; ++n) {
    printf("%02x ", b[n]);
-    if (n%16 == 15)
+    if (n % 16 == 15) {
      printf("\n");
    }
  }
-  if (n%16 != 0)
+  if (n % 16 != 0) {
    printf("\n");
  }
 }
 #endif
-void ahrs_propagate(float dt __attribute__((unused))) {
+void ahrs_ardrone2_propagate(void)
 {
  int l;
  //Recieve the main packet
  l = at_com_recieve_navdata(buffer);
-  navdata_t* main_packet = (navdata_t*) &buffer;
+  navdata_t *main_packet = (navdata_t *) &buffer;
 #ifdef ARDRONE2_DEBUG
-  if (l < 0)
+  if (l < 0) {
    printf("errno = %d\n", errno);
  }
 #endif
  //When this isn't a valid packet return
-  if(l < 0 || main_packet->header != NAVDATA_HEADER)
+  if (l < 0 || main_packet->header != NAVDATA_HEADER) {
    return;
  }
 #ifdef ARDRONE2_DEBUG
  printf("Read %d\n", l);
@@ -122,81 +131,75 @@ 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]);
+  navdata_option_t *navdata_option = (navdata_option_t *) & (main_packet->options[0]);
  bool_t full_read = FALSE;
  //The possible packets
-  navdata_demo_t* navdata_demo;
+  navdata_demo_t *navdata_demo;
-  navdata_gps_t* navdata_gps;
+  navdata_gps_t *navdata_gps;
-  navdata_phys_measures_t* navdata_phys_measures;
+  navdata_phys_measures_t *navdata_phys_measures;
  //Read the navdata until packet is fully readed
-  while(!full_read && navdata_option->size > 0) {
+  while (!full_read && navdata_option->size > 0) {
 #ifdef ARDRONE2_DEBUG
-    printf ("tag = %d\n", navdata_option->tag);
+    printf("tag = %d\n", navdata_option->tag);
 #endif
    //Check the tag for the right option
-    switch(navdata_option->tag) {
+    switch (navdata_option->tag) {
-    case 0: //NAVDATA_DEMO
+      case 0: //NAVDATA_DEMO
-      navdata_demo = (navdata_demo_t*) navdata_option;
+        navdata_demo = (navdata_demo_t *) navdata_option;
-      //Set the AHRS state
+        //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
+        //Set the ned to body eulers
-      struct FloatEulers angles;
+        struct FloatEulers angles;
-      angles.theta = navdata_demo->theta/180000.*M_PI;
+        angles.theta = navdata_demo->theta / 180000.*M_PI;
-      angles.psi = navdata_demo->psi/180000.*M_PI;
+        angles.psi = navdata_demo->psi / 180000.*M_PI;
-      angles.phi = navdata_demo->phi/180000.*M_PI;
+        angles.phi = navdata_demo->phi / 180000.*M_PI;
-      stateSetNedToBodyEulers_f(&angles);
+        stateSetNedToBodyEulers_f(&angles);
-      //Update the electrical supply
+        //Update the electrical supply
-      electrical.vsupply = navdata_demo->vbat_flying_percentage;
+        electrical.vsupply = navdata_demo->vbat_flying_percentage;
-      break;
+        break;
-    case 3: //NAVDATA_PHYS_MEASURES
+      case 3: //NAVDATA_PHYS_MEASURES
-      navdata_phys_measures = (navdata_phys_measures_t*) navdata_option;
+        navdata_phys_measures = (navdata_phys_measures_t *) navdata_option;
-      //Set the AHRS accel state
+        //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;
+        break;
 #ifdef USE_GPS_ARDRONE2
-    case 27: //NAVDATA_GPS
+      case 27: //NAVDATA_GPS
 # ifdef ARDRONE2_DEBUG
-      dump(navdata_option, navdata_option->size);
+        dump(navdata_option, navdata_option->size);
 # endif
-      navdata_gps = (navdata_gps_t*) navdata_option;
+        navdata_gps = (navdata_gps_t *) navdata_option;
-      // Send the data to the gps parser
+        // Send the data to the gps parser
-      gps_ardrone2_parse(navdata_gps);
+        gps_ardrone2_parse(navdata_gps);
-      break;
+        break;
 #endif
-    case 0xFFFF: //CHECKSUM
+      case 0xFFFF: //CHECKSUM
-      //TODO: Check the checksum
+        //TODO: Check the checksum
-      full_read = TRUE;
+        full_read = TRUE;
-      break;
+        break;
-    default:
+      default:
 #ifdef ARDRONE2_DEBUG
-      printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
+        printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
 #endif
-      break;
+        break;
    }
-    navdata_option = (navdata_option_t*) ((uint32_t)navdata_option + navdata_option->size);
+    navdata_option = (navdata_option_t *)((uint32_t)navdata_option + navdata_option->size);
  }
 }
 void ahrs_aligner_init(void) {
 }
 void ahrs_aligner_run(void) {
 }
@@ -30,10 +30,9 @@
 #ifndef AHRS_ARDRONE2_H
 #define AHRS_ARDRONE2_H
 #include "subsystems/ahrs.h"
 #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
@@ -44,7 +43,14 @@ struct AhrsARDrone {
  int32_t altitude;           // in cm above ground
  uint32_t battery;           // in percentage
  struct Int32Quat ltp_to_imu_quat;
  bool_t is_aligned;
 };
-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(void);
 extern void ahrs_ardrone2_propagate(void);
 #endif /* AHRS_ARDRONE2_H */
@@ -27,10 +27,17 @@
 * Propagation can be done in rotation matrix or quaternion representation.
 */
-#ifndef AHRS_FLOAT_CMPL
+#ifndef AHRS_FLOAT_CMPL_H
-#define AHRS_FLOAT_CMPL
+#define AHRS_FLOAT_CMPL_H
 #include "std.h"
 #include "math/pprz_algebra_float.h"
 #include "math/pprz_orientation_conversion.h"
 enum AhrsFCStatus {
  AHRS_FC_UNINIT,
  AHRS_FC_RUNNING
 };
 struct AhrsFloatCmpl {
  struct FloatRates gyro_bias;
@@ -60,23 +67,37 @@ struct AhrsFloatCmpl {
  /* internal counters for the gains */
  uint16_t accel_cnt; ///< number of propagations since last accel update
  uint16_t mag_cnt;   ///< number of propagations since last mag update
  struct OrientationReps body_to_imu;
  enum AhrsFCStatus status;
  bool_t is_aligned;
 };
-extern struct AhrsFloatCmpl ahrs_impl;
+extern struct AhrsFloatCmpl ahrs_fc;
 extern void ahrs_fc_init(void);
 extern void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu);
 extern void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i);
 extern bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
                            struct Int32Vect3 *lp_mag);
 extern void ahrs_fc_propagate(struct Int32Rates *gyro, float dt);
 extern void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt);
 extern void ahrs_fc_update_mag(struct Int32Vect3 *mag, float dt);
 extern void ahrs_fc_update_gps(void);
 /** Update yaw based on a heading measurement.
 * e.g. from GPS course
 * @param heading Heading in body frame, radians (CW/north)
 */
-void ahrs_update_heading(float heading);
+void ahrs_fc_update_heading(float heading);
 /** Hard reset yaw to a heading.
 * Doesn't affect the bias.
- * Sets ahrs_impl.heading_aligned to TRUE.
+ * Sets ahrs_fc.heading_aligned to TRUE.
 * @param heading Heading in body frame, radians (CW/north)
 */
-void ahrs_realign_heading(float heading);
+void ahrs_fc_realign_heading(float heading);
-#endif /* AHRS_FLOAT_CMPL_RMAT */
+#endif /* AHRS_FLOAT_CMPL_H */
@@ -0,0 +1,173 @@
 /*
 * Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
 *
 * 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, see
 * <http://www.gnu.org/licenses/>.
 */
 /**
 * @file subsystems/ahrs/ahrs_float_cmpl_wrapper.c
 *
 * Paparazzi specific wrapper to run floating point complementary filter.
 */
 #include "subsystems/ahrs/ahrs_float_cmpl_wrapper.h"
 #include "subsystems/ahrs.h"
 #include "subsystems/abi.h"
 #include "state.h"
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
 static void send_att(struct transport_tx *trans, struct link_device *dev)
 {
  struct FloatEulers ltp_to_imu_euler;
  float_eulers_of_quat(&ltp_to_imu_euler, &ahrs_fc.ltp_to_imu_quat);
  struct Int32Eulers euler_i;
  EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler);
  struct Int32Eulers *eulers_body = stateGetNedToBodyEulers_i();
  pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
                               &euler_i.phi,
                               &euler_i.theta,
                               &euler_i.psi,
                               &(eulers_body->phi),
                               &(eulers_body->theta),
                               &(eulers_body->psi));
 }
 static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
 {
  pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
                        &ahrs_fc.mag_h.x, &ahrs_fc.mag_h.y, &ahrs_fc.mag_h.z);
 }
 #endif
 /** 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 abi_event aligner_ev;
 static abi_event body_to_imu_ev;
 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
                    uint32_t __attribute__((unused)) stamp,
                    struct Int32Rates *gyro)
 {
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
  PRINT_CONFIG_MSG("Calculating dt for AHRS_FC propagation.")
  /* timestamp in usec when last callback was received */
  static uint32_t last_stamp = 0;
  if (last_stamp > 0 && ahrs_fc.is_aligned) {
    float dt = (float)(stamp - last_stamp) * 1e-6;
    ahrs_fc_propagate(gyro, dt);
  }
  last_stamp = stamp;
 #else
  PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_FC propagation.")
  PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
  if (ahrs_fc.status == AHRS_FC_RUNNING) {
    const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
    ahrs_fc_propagate(gyro, dt);
  }
 #endif
 }
 static void accel_cb(uint8_t __attribute__((unused)) sender_id,
                     uint32_t __attribute__((unused)) stamp,
                     struct Int32Vect3 *accel)
 {
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
  PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl accel update.")
  static uint32_t last_stamp = 0;
  if (last_stamp > 0 && ahrs_fc.is_aligned) {
    float dt = (float)(stamp - last_stamp) * 1e-6;
    ahrs_fc_update_accel((struct Int32Vect3 *)accel, dt);
  }
  last_stamp = stamp;
 #else
  PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS float_cmpl accel update.")
  PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
  if (ahrs_fc.is_aligned) {
    const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
    ahrs_fc_update_accel((struct Int32Vect3 *)accel, dt);
  }
 #endif
 }
 static void mag_cb(uint8_t __attribute__((unused)) sender_id,
                   uint32_t __attribute__((unused)) stamp,
                   struct Int32Vect3 *mag)
 {
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
  PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl mag update.")
  static uint32_t last_stamp = 0;
  if (last_stamp > 0 && ahrs_fc.is_aligned) {
    float dt = (float)(stamp - last_stamp) * 1e-6;
    ahrs_fc_update_mag(mag, dt);
  }
  last_stamp = stamp;
 #else
  PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS float_cmpl mag update.")
  PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
  if (ahrs_fc.is_aligned) {
    const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
    ahrs_fc_update_mag(mag, dt);
  }
 #endif
 }
 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
                       uint32_t stamp __attribute__((unused)),
                       struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
                       struct Int32Vect3 *lp_mag)
 {
  if (!ahrs_fc.is_aligned) {
    ahrs_fc_align(lp_gyro, lp_accel, lp_mag);
  }
 }
 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
                           struct FloatQuat *q_b2i_f)
 {
  ahrs_fc_set_body_to_imu_quat(q_b2i_f);
 }
 void ahrs_fc_register(void)
 {
  ahrs_register_impl(ahrs_fc_init, ahrs_fc_update_gps);
  /*
   * 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);
  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, "AHRS_EULER_INT", send_att);
  register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
 #endif
 }
@@ -0,0 +1,36 @@
 /*
 * Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
 *
 * 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, see
 * <http://www.gnu.org/licenses/>.
 */
 /**
 * @file subsystems/ahrs/ahrs_float_cmpl_wrapper.h
 *
 * Paparazzi specific wrapper to run floating point complementary filter.
 */
 #ifndef AHRS_FLOAT_CMPL_WRAPPER_H
 #define AHRS_FLOAT_CMPL_WRAPPER_H
 #include "subsystems/ahrs/ahrs_float_cmpl.h"
 #define DefaultAhrsImpl ahrs_fc
 extern void ahrs_fc_register(void);
 #endif /* AHRS_FLOAT_CMPL_WRAPPER_H */
@@ -26,6 +26,12 @@
 #include <inttypes.h>
 #include "math/pprz_algebra_float.h"
 #include "math/pprz_orientation_conversion.h"
 enum AhrsDCMStatus {
  AHRS_DCM_UNINIT,
  AHRS_DCM_RUNNING
 };
 struct AhrsFloatDCM {
  struct FloatRates gyro_bias;
@@ -39,9 +45,13 @@ struct AhrsFloatDCM {
  float gps_course;
  bool_t gps_course_valid;
  uint8_t gps_age;
 };
 extern struct AhrsFloatDCM ahrs_impl;
  struct OrientationReps body_to_imu;
  enum AhrsDCMStatus status;
  bool_t is_aligned;
 };
 extern struct AhrsFloatDCM ahrs_dcm;
 // DCM Parameters
@@ -69,4 +79,14 @@ extern int renorm_blowup_count;
 extern float imu_health;
 #endif
 extern void ahrs_dcm_init(void);
 extern void ahrs_dcm_set_body_to_imu(struct OrientationReps *body_to_imu);
 extern void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat *q_b2i);
 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);
 extern void ahrs_dcm_update_mag(struct Int32Vect3 *mag);
 extern void ahrs_dcm_update_gps(void);
 #endif // AHRS_FLOAT_DCM_H
@@ -21,34 +21,34 @@
 #define AHRS_FLOAT_DCM_ALGEBRA_H
-static inline float Vector_Dot_Product(float vector1[3],float vector2[3])
+static inline float Vector_Dot_Product(float vector1[3], float vector2[3])
 {
-  return vector1[0]*vector2[0] + vector1[1]*vector2[1] + vector1[2]*vector2[2];
+  return vector1[0] * vector2[0] + vector1[1] * vector2[1] + vector1[2] * vector2[2];
 }
-static inline void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
+static inline void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3])
 {
-  vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
+  vectorOut[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]);
-  vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
+  vectorOut[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]);
-  vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
+  vectorOut[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]);
 }
-static inline void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
+static inline void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2)
 {
-  vectorOut[0]=vectorIn[0]*scale2;
+  vectorOut[0] = vectorIn[0] * scale2;
-  vectorOut[1]=vectorIn[1]*scale2;
+  vectorOut[1] = vectorIn[1] * scale2;
-  vectorOut[2]=vectorIn[2]*scale2;
+  vectorOut[2] = vectorIn[2] * scale2;
 }
-static inline void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
+static inline void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3])
 {
-  vectorOut[0]=vectorIn1[0]+vectorIn2[0];
+  vectorOut[0] = vectorIn1[0] + vectorIn2[0];
-  vectorOut[1]=vectorIn1[1]+vectorIn2[1];
+  vectorOut[1] = vectorIn1[1] + vectorIn2[1];
-  vectorOut[2]=vectorIn1[2]+vectorIn2[2];
+  vectorOut[2] = vectorIn1[2] + vectorIn2[2];
 }
 /*
-  #define Matrix_Multiply( _m_a2b, _m_b2c, _m_a2c) {			\
+  #define Matrix_Multiply( _m_a2b, _m_b2c, _m_a2c) {      \
  _m_a2c[0] = (_m_b2c[0]*_m_a2b[0] + _m_b2c[1]*_m_a2b[3] + _m_b2c[2]*_m_a2b[6]); \
  _m_a2c[1] = (_m_b2c[0]*_m_a2b[1] + _m_b2c[1]*_m_a2b[4] + _m_b2c[2]*_m_a2b[7]); \
  _m_a2c[2] = (_m_b2c[0]*_m_a2b[2] + _m_b2c[1]*_m_a2b[5] + _m_b2c[2]*_m_a2b[8]); \
@@ -61,20 +61,17 @@ static inline void Vector_Add(float vectorOut[3],float vectorIn1[3], float vecto
  }
 */
-static inline void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
+static inline void Matrix_Multiply(float a[3][3], float b[3][3], float mat[3][3])
 {
  float op[3];
-  for(int x=0; x<3; x++)
+  for (int x = 0; x < 3; x++) {
-    {
+    for (int y = 0; y < 3; y++) {
-      for(int y=0; y<3; y++)
+      for (int w = 0; w < 3; w++) {
-        {
+        op[w] = a[x][w] * b[w][y];
-          for(int w=0; w<3; w++)
+      }
-            {
+      mat[x][y] = op[0] + op[1] + op[2];
              op[w]=a[x][w]*b[w][y];
            }
          mat[x][y]=op[0]+op[1]+op[2];
        }
    }
  }
 }
 #endif // AHRS_FLOAT_DCM_ALGEBRA_H
@@ -0,0 +1,114 @@
 /*
 * Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
 *
 * 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, see
 * <http://www.gnu.org/licenses/>.
 */
 /**
 * @file subsystems/ahrs/ahrs_float_dcm_wrapper.c
 *
 * Paparazzi specific wrapper to run floating point complementary filter.
 */
 #include "subsystems/ahrs/ahrs_float_dcm_wrapper.h"
 #include "subsystems/ahrs.h"
 #include "subsystems/abi.h"
 #include "state.h"
 /** 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 abi_event aligner_ev;
 static abi_event body_to_imu_ev;
 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
                    uint32_t __attribute__((unused)) stamp,
                    struct Int32Rates *gyro)
 {
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
  PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.")
  /* timestamp in usec when last callback was received */
  static uint32_t last_stamp = 0;
  if (last_stamp > 0 && ahrs_dcm.is_aligned) {
    float dt = (float)(stamp - last_stamp) * 1e-6;
    ahrs_dcm_propagate(gyro, dt);
  }
  last_stamp = stamp;
 #else
  PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS dcm propagation.")
  PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
  if (ahrs_dcm.is_aligned) {
    const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
    ahrs_dcm_propagate(gyro, dt);
  }
 #endif
 }
 static void accel_cb(uint8_t sender_id __attribute__((unused)),
                     uint32_t stamp __attribute__((unused)),
                     struct Int32Vect3 *accel)
 {
  if (ahrs_dcm.is_aligned) {
    ahrs_dcm_update_accel(accel);
  }
 }
 static void mag_cb(uint8_t sender_id __attribute__((unused)),
                   uint32_t stamp __attribute__((unused)),
                   struct Int32Vect3 *mag)
 {
  if (ahrs_dcm.is_aligned) {
    ahrs_dcm_update_mag(mag);
  }
 }
 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
                       uint32_t stamp __attribute__((unused)),
                       struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
                       struct Int32Vect3 *lp_mag)
 {
  if (!ahrs_dcm.is_aligned) {
    ahrs_dcm_align(lp_gyro, lp_accel, lp_mag);
  }
 }
 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
                           struct FloatQuat *q_b2i_f)
 {
  ahrs_dcm_set_body_to_imu_quat(q_b2i_f);
 }
 void ahrs_dcm_register(void)
 {
  ahrs_register_impl(ahrs_dcm_init, ahrs_dcm_update_gps);
  /*
   * 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);
  AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
  AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
 }
@@ -0,0 +1,36 @@
 /*
 * Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
 *
 * 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, see
 * <http://www.gnu.org/licenses/>.
 */
 /**
 * @file subsystems/ahrs/ahrs_float_dcm_wrapper.h
 *
 * Paparazzi specific wrapper to run floating point DCM filter.
 */
 #ifndef AHRS_FLOAT_DCM_WRAPPER_H
 #define AHRS_FLOAT_DCM_WRAPPER_H
 #include "subsystems/ahrs/ahrs_float_dcm.h"
 #define DefaultAhrsImpl ahrs_dcm
 extern void ahrs_dcm_register(void);
 #endif /* AHRS_FLOAT_DCM_WRAPPER_H */
@@ -29,16 +29,12 @@
 */
 #include "subsystems/ahrs/ahrs_float_mlkf.h"
 #include "subsystems/ahrs/ahrs_aligner.h"
 #include "subsystems/ahrs/ahrs_float_utils.h"
 #include <float.h>   /* for FLT_MIN     */
 #include <string.h>  /* for memcpy      */
 #include <math.h>    /* for M_PI        */
 #include "state.h"
 #include "subsystems/imu.h"
 #include "math/pprz_algebra_float.h"
 #include "math/pprz_algebra_int.h"
 #include "math/pprz_simple_matrix.h"
@@ -52,119 +48,153 @@
 #define AHRS_MAG_NOISE_Z 0.2
 #endif
-static inline void propagate_ref(float dt);
+static inline void propagate_ref(struct Int32Rates *gyro, float dt);
 static inline void propagate_state(float dt);
-static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise);
+static inline void update_state(const struct FloatVect3 *i_expected,
                                struct FloatVect3 *b_measured,
                                struct FloatVect3 *noise);
 static inline void update_state_heading(const struct FloatVect3 *i_expected,
                                        struct FloatVect3 *b_measured,
                                        struct FloatVect3 *noise);
 static inline void reset_state(void);
 static inline void set_body_state_from_quat(void);
-struct AhrsMlkf ahrs_impl;
+struct AhrsMlkf ahrs_mlkf;
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
-static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) {
+void ahrs_mlkf_init(void)
-  pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
+{
                        &ahrs_impl.mag_h.x, &ahrs_impl.mag_h.y, &ahrs_impl.mag_h.z);
 }
 #endif
-void ahrs_init(void) {
+  ahrs_mlkf.is_aligned = FALSE;
-  ahrs.status = AHRS_UNINIT;
+  /* 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);
-  /* Set ltp_to_imu so that body is zero */
+  VECT3_ASSIGN(ahrs_mlkf.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
  memcpy(&ahrs_impl.ltp_to_imu_quat, orientationGetQuat_f(&imu.body_to_imu),
         sizeof(struct FloatQuat));
  FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
  VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
  /*
   * Initialises our state
   */
-  FLOAT_RATES_ZERO(ahrs_impl.gyro_bias);
+  FLOAT_RATES_ZERO(ahrs_mlkf.gyro_bias);
  const float P0_a = 1.;
  const float P0_b = 1e-4;
  float P0[6][6] = {{ P0_a, 0.,   0.,   0.,   0.,   0.  },
-                    { 0.,   P0_a, 0.,   0.,   0.,   0.  },
+    { 0.,   P0_a, 0.,   0.,   0.,   0.  },
-                    { 0.,   0.,   P0_a, 0.,   0.,   0.  },
+    { 0.,   0.,   P0_a, 0.,   0.,   0.  },
-                    { 0.,   0.,   0.,   P0_b, 0.,   0.  },
+    { 0.,   0.,   0.,   P0_b, 0.,   0.  },
-                    { 0.,   0.,   0.,   0.,   P0_b, 0.  },
+    { 0.,   0.,   0.,   0.,   P0_b, 0.  },
-                    { 0.,   0.,   0.,   0.,   0.,   P0_b}};
+    { 0.,   0.,   0.,   0.,   0.,   P0_b}
-  memcpy(ahrs_impl.P, P0, sizeof(P0));
+  };
  memcpy(ahrs_mlkf.P, P0, sizeof(P0));
-  VECT3_ASSIGN(ahrs_impl.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z);
+  VECT3_ASSIGN(ahrs_mlkf.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z);
 #if PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
 #endif
 }
-void ahrs_align(void) {
+void ahrs_mlkf_set_body_to_imu(struct OrientationReps *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.is_aligned) {
    /* 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)
 {
  /* Compute an initial orientation from accel and mag directly as quaternion */
-  ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
+  ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_imu_quat, lp_accel, lp_mag);
  /* set initial body orientation */
  set_body_state_from_quat();
  /* used averaged gyro as initial value for bias */
  struct Int32Rates bias0;
-  RATES_COPY(bias0, ahrs_aligner.lp_gyro);
+  RATES_COPY(bias0, *lp_gyro);
-  RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);
+  RATES_FLOAT_OF_BFP(ahrs_mlkf.gyro_bias, bias0);
-  ahrs.status = AHRS_RUNNING;
+  ahrs_mlkf.is_aligned = TRUE;
  return TRUE;
 }
-void ahrs_propagate(float dt) {
+void ahrs_mlkf_propagate(struct Int32Rates *gyro, float dt)
-  propagate_ref(dt);
+{
  propagate_ref(gyro, dt);
  propagate_state(dt);
  set_body_state_from_quat();
 }
-void ahrs_update_accel(float dt __attribute__((unused))) {
+void ahrs_mlkf_update_accel(struct Int32Vect3 *accel)
 {
  struct FloatVect3 imu_g;
-  ACCELS_FLOAT_OF_BFP(imu_g, imu.accel);
+  ACCELS_FLOAT_OF_BFP(imu_g, *accel);
  const float alpha = 0.92;
-  ahrs_impl.lp_accel = alpha * ahrs_impl.lp_accel +
+  ahrs_mlkf.lp_accel = alpha * ahrs_mlkf.lp_accel +
-    (1. - alpha) *(float_vect3_norm(&imu_g) - 9.81);
+                       (1. - alpha) * (float_vect3_norm(&imu_g) - 9.81);
  const struct FloatVect3 earth_g = {0.,  0., -9.81 };
-  const float dn = 250*fabs( ahrs_impl.lp_accel );
+  const float dn = 250 * fabs(ahrs_mlkf.lp_accel);
-  struct FloatVect3 g_noise = {1.+dn, 1.+dn, 1.+dn};
+  struct FloatVect3 g_noise = {1. + dn, 1. + dn, 1. + dn};
  update_state(&earth_g, &imu_g, &g_noise);
  reset_state();
 }
 void ahrs_mlkf_update_mag(struct Int32Vect3 *mag)
 {
 #if AHRS_MAG_UPDATE_ALL_AXES
  ahrs_mlkf_update_mag_full(mag);
 #else
  ahrs_mlkf_update_mag_2d(mag);
 #endif
 }
-void ahrs_update_mag(float dt __attribute__((unused))) {
+void ahrs_mlkf_update_mag_2d(struct Int32Vect3 *mag)
 {
  struct FloatVect3 imu_h;
-  MAGS_FLOAT_OF_BFP(imu_h, imu.mag);
+  MAGS_FLOAT_OF_BFP(imu_h, *mag);
-  update_state(&ahrs_impl.mag_h, &imu_h, &ahrs_impl.mag_noise);
+  update_state_heading(&ahrs_mlkf.mag_h, &imu_h, &ahrs_mlkf.mag_noise);
  reset_state();
 }
 void ahrs_mlkf_update_mag_full(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);
  reset_state();
 }
-static inline void propagate_ref(float dt) {
+static inline void propagate_ref(struct Int32Rates *gyro, float dt)
 {
  /* converts gyro to floating point */
  struct FloatRates gyro_float;
-  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
+  RATES_FLOAT_OF_BFP(gyro_float, *gyro);
  /* unbias measurement */
-  RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
+  RATES_SUB(gyro_float, ahrs_mlkf.gyro_bias);
 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
  /* lowpass angular rates */
  const float alpha = 0.1;
-  FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate,
+  FLOAT_RATES_LIN_CMB(ahrs_mlkf.imu_rate, ahrs_mlkf.imu_rate,
-                      (1.-alpha), gyro_float, alpha);
+                      (1. - alpha), gyro_float, alpha);
 #else
-  RATES_COPY(ahrs_impl.imu_rate, gyro_float);
+  RATES_COPY(ahrs_mlkf.imu_rate, gyro_float);
 #endif
  /* propagate reference quaternion */
-  float_quat_integrate(&ahrs_impl.ltp_to_imu_quat, &ahrs_impl.imu_rate, dt);
+  float_quat_integrate(&ahrs_mlkf.ltp_to_imu_quat, &ahrs_mlkf.imu_rate, dt);
 }
@@ -172,48 +202,57 @@ static inline void propagate_ref(float dt) {
 * Progagate filter's covariance
 * We don't propagate state as we assume to have reseted
 */
-static inline void propagate_state(float dt) {
+static inline void propagate_state(float dt)
 {
  /* predict covariance */
-  const float dp = ahrs_impl.imu_rate.p*dt;
+  const float dp = ahrs_mlkf.imu_rate.p * dt;
-  const float dq = ahrs_impl.imu_rate.q*dt;
+  const float dq = ahrs_mlkf.imu_rate.q * dt;
-  const float dr = ahrs_impl.imu_rate.r*dt;
+  const float dr = ahrs_mlkf.imu_rate.r * dt;
  float F[6][6] = {{  1.,   dr,  -dq,  -dt,   0.,   0.  },
-                   { -dr,   1.,   dp,   0.,  -dt,   0.  },
+    { -dr,   1.,   dp,   0.,  -dt,   0.  },
-                   {  dq,  -dp,   1.,   0.,   0.,  -dt  },
+    {  dq,  -dp,   1.,   0.,   0.,  -dt  },
-                   {  0.,   0.,   0.,   1.,   0.,   0.  },
+    {  0.,   0.,   0.,   1.,   0.,   0.  },
-                   {  0.,   0.,   0.,   0.,   1.,   0.  },
+    {  0.,   0.,   0.,   0.,   1.,   0.  },
-                   {  0.,   0.,   0.,   0.,   0.,   1.  }};
+    {  0.,   0.,   0.,   0.,   0.,   1.  }
  };
  // P = FPF' + GQG
  float tmp[6][6];
-  MAT_MUL(6,6,6, tmp, F, ahrs_impl.P);
+  MAT_MUL(6, 6, 6, tmp, F, ahrs_mlkf.P);
-  MAT_MUL_T(6,6,6,  ahrs_impl.P, tmp, F);
+  MAT_MUL_T(6, 6, 6,  ahrs_mlkf.P, tmp, F);
  const float dt2 = dt * dt;
-  const float GQG[6] = {dt2*10e-3, dt2*10e-3, dt2*10e-3, dt2*9e-6, dt2*9e-6, dt2*9e-6 };
+  const float GQG[6] = {dt2 * 10e-3, dt2 * 10e-3, dt2 * 10e-3, dt2 * 9e-6, dt2 * 9e-6, dt2 * 9e-6 };
-  for (int i=0;i<6;i++)
+  for (int i = 0; i < 6; i++) {
-    ahrs_impl.P[i][i] += GQG[i];
+    ahrs_mlkf.P[i][i] += GQG[i];
  }
 }
 /**
- *  Incorporate one 3D vector measurement
+ * Incorporate one 3D vector measurement.
 * @param i_expected expected 3d vector in inertial frame
 * @param b_measured measured 3d vector in body/imu frame
 * @param noise measurement noise vector (diagonal of covariance)
 */
-static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise) {
+static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured,
                                struct FloatVect3 *noise)
 {
  /* converted expected measurement from inertial to body frame */
  struct FloatVect3 b_expected;
-  float_quat_vmult(&b_expected, &ahrs_impl.ltp_to_imu_quat, (struct FloatVect3*)i_expected);
+  float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, (struct FloatVect3 *)i_expected);
  // S = HPH' + JRJ
  float H[3][6] = {{           0., -b_expected.z,  b_expected.y, 0., 0., 0.},
                   { b_expected.z,            0., -b_expected.x, 0., 0., 0.},
-                   {-b_expected.y,  b_expected.x,            0., 0., 0., 0.}};
+                   { -b_expected.y, b_expected.x,            0., 0., 0., 0.}
  };
  float tmp[3][6];
-  MAT_MUL(3,6,6, tmp, H, ahrs_impl.P);
+  MAT_MUL(3, 6, 6, tmp, H, ahrs_mlkf.P);
  float S[3][3];
-  MAT_MUL_T(3,6,3, S, tmp, H);
+  MAT_MUL_T(3, 6, 3, S, tmp, H);
  /* add the measurement noise */
  S[0][0] += noise->x;
@@ -225,68 +264,142 @@ static inline void update_state(const struct FloatVect3 *i_expected, struct Floa
  // K = PH'invS
  float tmp2[6][3];
-  MAT_MUL_T(6,6,3, tmp2, ahrs_impl.P, H);
+  MAT_MUL_T(6, 6, 3, tmp2, ahrs_mlkf.P, H);
  float K[6][3];
-  MAT_MUL(6,3,3, K, tmp2, invS);
+  MAT_MUL(6, 3, 3, K, tmp2, invS);
  // P = (I-KH)P
  float tmp3[6][6];
-  MAT_MUL(6,3,6, tmp3, K, H);
+  MAT_MUL(6, 3, 6, tmp3, K, H);
  float I6[6][6] = {{ 1., 0., 0., 0., 0., 0. },
-                    {  0., 1., 0., 0., 0., 0. },
+    {  0., 1., 0., 0., 0., 0. },
-                    {  0., 0., 1., 0., 0., 0. },
+    {  0., 0., 1., 0., 0., 0. },
-                    {  0., 0., 0., 1., 0., 0. },
+    {  0., 0., 0., 1., 0., 0. },
-                    {  0., 0., 0., 0., 1., 0. },
+    {  0., 0., 0., 0., 1., 0. },
-                    {  0., 0., 0., 0., 0., 1. }};
+    {  0., 0., 0., 0., 0., 1. }
  };
  float tmp4[6][6];
-  MAT_SUB(6,6, tmp4, I6, tmp3);
+  MAT_SUB(6, 6, tmp4, I6, tmp3);
  float tmp5[6][6];
-  MAT_MUL(6,6,6, tmp5, tmp4, ahrs_impl.P);
+  MAT_MUL(6, 6, 6, tmp5, tmp4, ahrs_mlkf.P);
-  memcpy(ahrs_impl.P, tmp5, sizeof(ahrs_impl.P));
+  memcpy(ahrs_mlkf.P, tmp5, sizeof(ahrs_mlkf.P));
  // X = X + Ke
  struct FloatVect3 e;
  VECT3_DIFF(e, *b_measured, b_expected);
-  ahrs_impl.gibbs_cor.qx  += K[0][0]*e.x + K[0][1]*e.y + K[0][2]*e.z;
+  ahrs_mlkf.gibbs_cor.qx += K[0][0] * e.x + K[0][1] * e.y + K[0][2] * e.z;
-  ahrs_impl.gibbs_cor.qy  += K[1][0]*e.x + K[1][1]*e.y + K[1][2]*e.z;
+  ahrs_mlkf.gibbs_cor.qy += K[1][0] * e.x + K[1][1] * e.y + K[1][2] * e.z;
-  ahrs_impl.gibbs_cor.qz  += K[2][0]*e.x + K[2][1]*e.y + K[2][2]*e.z;
+  ahrs_mlkf.gibbs_cor.qz += K[2][0] * e.x + K[2][1] * e.y + K[2][2] * e.z;
-  ahrs_impl.gyro_bias.p  += K[3][0]*e.x + K[3][1]*e.y + K[3][2]*e.z;
+  ahrs_mlkf.gyro_bias.p  += K[3][0] * e.x + K[3][1] * e.y + K[3][2] * e.z;
-  ahrs_impl.gyro_bias.q  += K[4][0]*e.x + K[4][1]*e.y + K[4][2]*e.z;
+  ahrs_mlkf.gyro_bias.q  += K[4][0] * e.x + K[4][1] * e.y + K[4][2] * e.z;
-  ahrs_impl.gyro_bias.r  += K[5][0]*e.x + K[5][1]*e.y + K[5][2]*e.z;
+  ahrs_mlkf.gyro_bias.r  += K[5][0] * e.x + K[5][1] * e.y + K[5][2] * e.z;
 }
 /**
 * Incorporate one 3D vector measurement, only correcting heading.
 * @param i_expected expected 3d vector in inertial frame
 * @param b_measured measured 3d vector in body/imu frame
 * @param noise measurement noise vector (diagonal of covariance)
 * TODO: optimize
 */
 static inline void update_state_heading(const struct FloatVect3 *i_expected,
                                        struct FloatVect3 *b_measured,
                                        struct FloatVect3 *noise)
 {
  /* converted expected measurement from inertial to body frame */
  struct FloatVect3 b_expected;
  float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, (struct FloatVect3 *)i_expected);
  /* set roll/pitch errors to zero to only correct heading */
  struct FloatVect3 i_h_2d = {i_expected->y, -i_expected->x, 0.f};
  struct FloatVect3 b_yaw;
  float_quat_vmult(&b_yaw, &ahrs_mlkf.ltp_to_imu_quat, &i_h_2d);
  // S = HPH' + JRJ
  float H[3][6] = {{ 0., 0., b_yaw.x, 0., 0., 0.},
                   { 0., 0., b_yaw.y, 0., 0., 0.},
                   { 0., 0., b_yaw.z, 0., 0., 0.}
  };
  float tmp[3][6];
  MAT_MUL(3, 6, 6, tmp, H, ahrs_mlkf.P);
  float S[3][3];
  MAT_MUL_T(3, 6, 3, S, tmp, H);
  /* add the measurement noise */
  S[0][0] += noise->x;
  S[1][1] += noise->y;
  S[2][2] += noise->z;
  float invS[3][3];
  MAT_INV33(invS, S);
  // K = PH'invS
  float tmp2[6][3];
  MAT_MUL_T(6, 6, 3, tmp2, ahrs_mlkf.P, H);
  float K[6][3];
  MAT_MUL(6, 3, 3, K, tmp2, invS);
  // P = (I-KH)P
  float tmp3[6][6];
  MAT_MUL(6, 3, 6, tmp3, K, H);
  float I6[6][6] = {{ 1., 0., 0., 0., 0., 0. },
    {  0., 1., 0., 0., 0., 0. },
    {  0., 0., 1., 0., 0., 0. },
    {  0., 0., 0., 1., 0., 0. },
    {  0., 0., 0., 0., 1., 0. },
    {  0., 0., 0., 0., 0., 1. }
  };
  float tmp4[6][6];
  MAT_SUB(6, 6, tmp4, I6, tmp3);
  float tmp5[6][6];
  MAT_MUL(6, 6, 6, tmp5, tmp4, ahrs_mlkf.P);
  memcpy(ahrs_mlkf.P, tmp5, sizeof(ahrs_mlkf.P));
  // X = X + Ke
  struct FloatVect3 e;
  VECT3_DIFF(e, *b_measured, b_expected);
  ahrs_mlkf.gibbs_cor.qx += K[0][0] * e.x + K[0][1] * e.y + K[0][2] * e.z;
  ahrs_mlkf.gibbs_cor.qy += K[1][0] * e.x + K[1][1] * e.y + K[1][2] * e.z;
  ahrs_mlkf.gibbs_cor.qz += K[2][0] * e.x + K[2][1] * e.y + K[2][2] * e.z;
  ahrs_mlkf.gyro_bias.p  += K[3][0] * e.x + K[3][1] * e.y + K[3][2] * e.z;
  ahrs_mlkf.gyro_bias.q  += K[4][0] * e.x + K[4][1] * e.y + K[4][2] * e.z;
  ahrs_mlkf.gyro_bias.r  += K[5][0] * e.x + K[5][1] * e.y + K[5][2] * e.z;
 }
 /**
 * Incorporate errors to reference and zeros state
 */
-static inline void reset_state(void) {
+static inline void reset_state(void)
 {
-  ahrs_impl.gibbs_cor.qi = 2.;
+  ahrs_mlkf.gibbs_cor.qi = 2.;
  struct FloatQuat q_tmp;
-  float_quat_comp(&q_tmp, &ahrs_impl.ltp_to_imu_quat, &ahrs_impl.gibbs_cor);
+  float_quat_comp(&q_tmp, &ahrs_mlkf.ltp_to_imu_quat, &ahrs_mlkf.gibbs_cor);
  float_quat_normalize(&q_tmp);
-  memcpy(&ahrs_impl.ltp_to_imu_quat, &q_tmp, sizeof(ahrs_impl.ltp_to_imu_quat));
+  memcpy(&ahrs_mlkf.ltp_to_imu_quat, &q_tmp, sizeof(ahrs_mlkf.ltp_to_imu_quat));
-  float_quat_identity(&ahrs_impl.gibbs_cor);
+  float_quat_identity(&ahrs_mlkf.gibbs_cor);
 }
 /**
 * Compute body orientation and rates from imu orientation and rates
 */
-static inline void set_body_state_from_quat(void) {
+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);
+  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_mlkf.body_to_imu);
  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_mlkf.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, body_to_imu_quat);
+  float_quat_comp_inv(&ltp_to_body_quat, &ahrs_mlkf.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, body_to_imu_rmat, &ahrs_impl.imu_rate);
+  float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_mlkf.imu_rate);
  /* Set state */
  stateSetBodyRates_f(&body_rate);
@@ -31,9 +31,14 @@
 #ifndef AHRS_FLOAT_MLKF_H
 #define AHRS_FLOAT_MLKF_H
 #include "subsystems/ahrs.h"
 #include "std.h"
 #include "math/pprz_algebra_float.h"
 #include "math/pprz_orientation_conversion.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
@@ -48,8 +53,25 @@ struct AhrsMlkf {
  struct FloatQuat  gibbs_cor;
  float P[6][6];
  float lp_accel;
  /** body_to_imu rotation */
  struct OrientationReps body_to_imu;
  enum AhrsMlkfStatus status;
  bool_t is_aligned;
 };
-extern struct AhrsMlkf ahrs_impl;
+extern struct AhrsMlkf ahrs_mlkf;
 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);
 extern void ahrs_mlkf_update_accel(struct Int32Vect3 *accel);
 extern void ahrs_mlkf_update_mag(struct Int32Vect3 *mag);
 extern void ahrs_mlkf_update_mag_2d(struct Int32Vect3 *mag);
 extern void ahrs_mlkf_update_mag_full(struct Int32Vect3 *mag);
 #endif /* AHRS_FLOAT_MLKF_H */
@@ -0,0 +1,130 @@
 /*
 * Copyright (C) 2014 Felix Ruess <felix.ruess@gmail.com>
 *
 * 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 subsystems/ahrs/ahrs_float_mlkf_wrapper.c
 *
 * Paparazzi specific wrapper to run MLKF filter.
 */
 #include "subsystems/ahrs/ahrs_float_mlkf_wrapper.h"
 #include "subsystems/ahrs.h"
 #include "subsystems/abi.h"
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
 static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
 {
  pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
                        &ahrs_mlkf.mag_h.x, &ahrs_mlkf.mag_h.y, &ahrs_mlkf.mag_h.z);
 }
 #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 abi_event aligner_ev;
 static abi_event body_to_imu_ev;
 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
                    uint32_t __attribute__((unused)) stamp,
                    struct Int32Rates *gyro)
 {
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
  PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.")
  /* timestamp in usec when last callback was received */
  static uint32_t last_stamp = 0;
  if (last_stamp > 0 && ahrs_mlkf.is_aligned) {
    float dt = (float)(stamp - last_stamp) * 1e-6;
    ahrs_mlkf_propagate(gyro, dt);
  }
  last_stamp = stamp;
 #else
  PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_MLKF propagation.")
  PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
  if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
    const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
    ahrs_mlkf_propagate(gyro, dt);
  }
 #endif
 }
 static void accel_cb(uint8_t sender_id __attribute__((unused)),
                     uint32_t stamp __attribute__((unused)),
                     struct Int32Vect3 *accel)
 {
  if (ahrs_mlkf.is_aligned) {
    ahrs_mlkf_update_accel(accel);
  }
 }
 static void mag_cb(uint8_t sender_id __attribute__((unused)),
                   uint32_t stamp __attribute__((unused)),
                   struct Int32Vect3 *mag)
 {
  if (ahrs_mlkf.is_aligned) {
    ahrs_mlkf_update_mag(mag);
  }
 }
 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
                       uint32_t stamp __attribute__((unused)),
                       struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
                       struct Int32Vect3 *lp_mag)
 {
  if (!ahrs_mlkf.is_aligned) {
    ahrs_mlkf_align(lp_accel, lp_accel, lp_mag);
  }
 }
 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
                           struct FloatQuat *q_b2i_f)
 {
  ahrs_mlkf_set_body_to_imu_quat(q_b2i_f);
 }
 void ahrs_mlkf_register(void)
 {
  ahrs_register_impl(ahrs_mlkf_init, NULL);
  /*
   * 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);
  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);
 #endif
 }
@@ -0,0 +1,36 @@
 /*
 * Copyright (C) 2014 Felix Ruess <felix.ruess@gmail.com>
 *
 * 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, see
 * <http://www.gnu.org/licenses/>.
 */
 /**
 * @file subsystems/ahrs/ahrs_float_mlkf_wrapper.h
 *
 * Paparazzi specific wrapper to run MLKF filter.
 */
 #ifndef AHRS_FLOAT_MLKF_WRAPPER_H
 #define AHRS_FLOAT_MLKF_WRAPPER_H
 #include "subsystems/ahrs/ahrs_float_mlkf.h"
 #define DefaultAhrsImpl ahrs_mlkf
 extern void ahrs_mlkf_register(void);
 #endif /* AHRS_FLOAT_MLKF_WRAPPER_H */
--- a/Show More
+++ b/Show More