diff --git a/conf/airframes/CDW/LisaAspirin2.xml b/conf/airframes/CDW/LisaAspirin2.xml
index fea5a463b7..aef11dfc20 100644
--- a/conf/airframes/CDW/LisaAspirin2.xml
+++ b/conf/airframes/CDW/LisaAspirin2.xml
@@ -209,7 +209,6 @@
       <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
 
       <configure name="AHRS_ALIGNER_LED" value="1"/>
-      <configure name="CPU_LED" value="1"/>
     </target>
     <target name="sim" board="pc"/>
 
diff --git a/conf/airframes/CDW/yapa3_aspirin2.xml b/conf/airframes/CDW/yapa3_aspirin2.xml
index f231dc2a82..edfe73741c 100644
--- a/conf/airframes/CDW/yapa3_aspirin2.xml
+++ b/conf/airframes/CDW/yapa3_aspirin2.xml
@@ -233,7 +233,6 @@
       <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
 
       <configure name="AHRS_ALIGNER_LED" value="3"/>
-      <configure name="CPU_LED" value="3"/>
     </target>
     <target name="sim" board="pc"/>
 
diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index 54308f5d48..54b0bc2af9 100644
--- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -225,7 +225,6 @@
       <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
 
       <configure name="AHRS_ALIGNER_LED" value="3"/>
-      <configure name="CPU_LED" value="3"/>
     </target>
     <target name="sim" board="pc"/>
 
diff --git a/conf/airframes/examples/microjet_imu_xsens.xml b/conf/airframes/examples/microjet_imu_xsens.xml
new file mode 100644
index 0000000000..c351c1181c
--- /dev/null
+++ b/conf/airframes/examples/microjet_imu_xsens.xml
@@ -0,0 +1,205 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<!-- Microjet Multiplex (http://www.multiplex-rc.de/)
+     Lisa/M v1.0 board (http://paparazzi.enac.fr/wiki/Lisa/M_v10)
+     Xsens running as IMU
+     Xbee modem in transparent mode
+-->
+
+<airframe name="Microjet Lisa/M 1.0">
+
+  <firmware name="fixedwing">
+    <target name="sim" 	board="pc"/>
+    <target name="ap"   board="lisa_m_1.0"/>
+
+    <define name="AGR_CLIMB" />
+    <define name="LOITER_TRIM" />
+    <define name="ALT_KALMAN" />
+
+    <subsystem name="radio_control" type="ppm"/>
+
+    <!-- Communication -->
+    <subsystem name="telemetry" 	type="transparent"/>
+
+    <!-- Sensors -->
+    <subsystem name="imu"           type="xsens">
+      <configure name="XSENS_UART_NR" value="3"/>
+      <configure name="XSENS_UART_BAUD" value="B115200"/>
+    </subsystem>
+    <subsystem name="ahrs" type="int_cmpl_quat"/>
+    <subsystem name="gps" type="ublox"/>
+    <subsystem name="control"/>
+    <subsystem name="navigation"/>
+  </firmware>
+
+  <modules>
+    <load name="sys_mon.xml"/>
+  </modules>
+
+  <servos>
+    <servo name="MOTOR"         no="0" min="1290" neutral="1290" max="1810"/>
+    <servo name="AILEVON_LEFT"  no="1" min="2000" neutral="1510" max="1000"/>
+    <servo name="AILEVON_RIGHT" no="3" min="1000" neutral="1535" max="2000"/>
+  </servos>
+
+  <commands>
+    <axis name="THROTTLE" failsafe_value="0"/>
+    <axis name="ROLL"     failsafe_value="0"/>
+    <axis name="PITCH"    failsafe_value="0"/>
+  </commands>
+
+  <rc_commands>
+    <set command="THROTTLE" value="@THROTTLE"/>
+    <set command="ROLL"     value="@ROLL"/>
+    <set command="PITCH"    value="@PITCH"/>
+  </rc_commands>
+
+  <section name="MIXER">
+    <define name="AILEVON_AILERON_RATE" value="0.75"/>
+    <define name="AILEVON_ELEVATOR_RATE" value="0.75"/>
+  </section>
+
+  <command_laws>
+    <let var="aileron"  value="@ROLL  * AILEVON_AILERON_RATE"/>
+    <let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
+    <set servo="MOTOR"         value="@THROTTLE"/>
+    <set servo="AILEVON_LEFT"  value="$elevator - $aileron"/>
+    <set servo="AILEVON_RIGHT" value="$elevator + $aileron"/>
+  </command_laws>
+
+  <section name="IMU" prefix="IMU_">
+
+    <define name="GYRO_P_SIGN"  value="1"/>
+    <define name="GYRO_Q_SIGN"  value="1"/>
+    <define name="GYRO_R_SIGN"  value="1"/>
+
+    <define name="GYRO_P_NEUTRAL" value="0"/>
+    <define name="GYRO_R_NEUTRAL" value="0"/>
+    <define name="GYRO_Q_NEUTRAL" value="0"/>
+
+    <define name="GYRO_P_SENS" value="1" integer="16"/>
+    <define name="GYRO_R_SENS" value="1" integer="16"/>
+    <define name="GYRO_Q_SENS" value="1" integer="16"/>
+
+    <define name="ACCEL_X_SIGN"  value="1"/>
+    <define name="ACCEL_Y_SIGN"  value="1"/>
+    <define name="ACCEL_Z_SIGN"  value="1"/>
+
+    <define name="ACCEL_X_SENS" value="1" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="1" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="1" integer="16"/>
+
+    <define name="ACCEL_X_NEUTRAL" value="0"/>
+    <define name="ACCEL_Z_NEUTRAL" value="0"/>
+    <define name="ACCEL_Y_NEUTRAL" value="0"/>
+
+    <define name="MAG_X_SIGN" value="1"/>
+    <define name="MAG_Y_SIGN" value="1"/>
+    <define name="MAG_Z_SIGN" value="1"/>
+
+    <define name="MAG_X_NEUTRAL" value="-45"/>
+    <define name="MAG_Y_NEUTRAL" value="334"/>
+    <define name="MAG_Z_NEUTRAL" value="7"/>
+
+    <define name="MAG_X_SENS" value="4.47647816128" integer="16"/>
+    <define name="MAG_Y_SENS" value="4.71085671542" integer="16"/>
+    <define name="MAG_Z_SENS" value="4.41585354498" integer="16"/>
+
+    <define name="BODY_TO_IMU_PHI" value="0" unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0" unit="deg"/>
+    <define name="BODY_TO_IMU_PSI" value="0" unit="deg"/>
+  </section>
+
+  <section name="AHRS" prefix="AHRS_">
+    <!-- replace this with your local magnetic field -->
+    <define name="H_X" value="0.3770441"/>
+    <define name="H_Y" value="0.0193986"/>
+    <define name="H_Z" value="0.9259921"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  </section>
+
+  <section name="VERTICAL CONTROL" prefix="V_CTL_">
+    <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
+    <!-- outer loop proportional gain -->
+    <define name="ALTITUDE_PGAIN" value="0.03"/>
+    <!-- outer loop saturation -->
+    <define name="ALTITUDE_MAX_CLIMB" value="2."/>
+
+    <!-- auto throttle inner loop -->
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
+    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
+    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="0.01"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
+
+    <define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
+  </section>
+
+  <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
+    <define name="COURSE_PGAIN" value="1.0"/>
+    <define name="COURSE_DGAIN" value="0.3"/>
+
+    <define name="ROLL_MAX_SETPOINT"  value="35"  unit="deg"/>
+    <define name="PITCH_MAX_SETPOINT" value="30"  unit="deg"/>
+    <define name="PITCH_MIN_SETPOINT" value="-30" unit="deg"/>
+
+    <define name="PITCH_PGAIN" value="12000."/>
+    <define name="PITCH_DGAIN" value="1.5"/>
+
+    <define name="ELEVATOR_OF_ROLL" value="1250"/>
+
+    <define name="ROLL_SLEW" value="0.1"/>
+
+    <define name="ROLL_ATTITUDE_GAIN" value="7500"/>
+    <define name="ROLL_RATE_GAIN" value="1500"/>
+  </section>
+
+  <section name="AGGRESSIVE" prefix="AGR_">
+    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
+    <define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
+    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
+    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="DESCENT_NAV_RATIO" value="1.0"/>
+    </section>
+
+  <section name="FAILSAFE" prefix="FAILSAFE_">
+    <define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
+    <define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
+    <define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
+    <define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
+  </section>
+
+  <section name="AUTO1" prefix="AUTO1_">
+    <define name="MAX_ROLL" value="40." unit="deg"/>
+    <define name="MAX_PITCH" value="35." unit="deg"/>
+  </section>
+
+  <section name="BAT">
+    <define name="MILLIAMP_AT_FULL_THROTTLE" value="2000"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
+    <define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
+    <define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
+  </section>
+
+  <section name="MISC">
+    <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
+    <define name="CARROT" value="5." unit="s"/>
+    <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
+    <define name="CONTROL_RATE" value="60" unit="Hz"/>
+    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+    <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
+  </section>
+
+</airframe>
diff --git a/conf/airframes/examples/microjet_lisa_m_xsens.xml b/conf/airframes/examples/microjet_lisa_m_xsens.xml
new file mode 100644
index 0000000000..b0c17f2f55
--- /dev/null
+++ b/conf/airframes/examples/microjet_lisa_m_xsens.xml
@@ -0,0 +1,160 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<!-- Microjet Multiplex (http://www.multiplex-rc.de/)
+     Lisa/M v1.0 board (http://paparazzi.enac.fr/wiki/Lisa/M_v10)
+     Xsens as full INS
+     Xbee modem in transparent mode
+-->
+
+<airframe name="Microjet Lisa/M 1.0">
+
+  <firmware name="fixedwing">
+    <target name="sim" 	board="pc"/>
+    <target name="ap"   board="lisa_m_1.0"/>
+
+    <define name="AGR_CLIMB" />
+    <define name="LOITER_TRIM" />
+    <define name="ALT_KALMAN" />
+
+    <subsystem name="radio_control" type="ppm"/>
+
+    <!-- Communication -->
+    <subsystem name="telemetry" 	type="transparent"/>
+
+    <!-- Sensors -->
+    <subsystem name="ins"           type="xsens">
+      <configure name="XSENS_UART_NR" value="3"/>
+      <configure name="XSENS_UART_BAUD" value="B115200"/>
+    </subsystem>
+
+    <subsystem name="control"/>
+    <subsystem name="navigation"/>
+  </firmware>
+
+  <modules>
+    <load name="sys_mon.xml"/>
+  </modules>
+
+  <servos>
+    <servo name="MOTOR"         no="0" min="1290" neutral="1290" max="1810"/>
+    <servo name="AILEVON_LEFT"  no="1" min="2000" neutral="1510" max="1000"/>
+    <servo name="AILEVON_RIGHT" no="3" min="1000" neutral="1535" max="2000"/>
+  </servos>
+
+  <commands>
+    <axis name="THROTTLE" failsafe_value="0"/>
+    <axis name="ROLL"     failsafe_value="0"/>
+    <axis name="PITCH"    failsafe_value="0"/>
+  </commands>
+
+  <rc_commands>
+    <set command="THROTTLE" value="@THROTTLE"/>
+    <set command="ROLL"     value="@ROLL"/>
+    <set command="PITCH"    value="@PITCH"/>
+  </rc_commands>
+
+  <section name="MIXER">
+    <define name="AILEVON_AILERON_RATE" value="0.75"/>
+    <define name="AILEVON_ELEVATOR_RATE" value="0.75"/>
+  </section>
+
+  <command_laws>
+    <let var="aileron"  value="@ROLL  * AILEVON_AILERON_RATE"/>
+    <let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
+    <set servo="MOTOR"         value="@THROTTLE"/>
+    <set servo="AILEVON_LEFT"  value="$elevator - $aileron"/>
+    <set servo="AILEVON_RIGHT" value="$elevator + $aileron"/>
+  </command_laws>
+
+  <section name="IMU" prefix="IMU_">
+    <define name="BODY_TO_IMU_PHI" value="0" unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0" unit="deg"/>
+    <define name="BODY_TO_IMU_PSI" value="0" unit="deg"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  </section>
+
+  <section name="VERTICAL CONTROL" prefix="V_CTL_">
+    <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
+    <!-- outer loop proportional gain -->
+    <define name="ALTITUDE_PGAIN" value="0.03"/>
+    <!-- outer loop saturation -->
+    <define name="ALTITUDE_MAX_CLIMB" value="2."/>
+
+    <!-- auto throttle inner loop -->
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
+    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
+    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="0.01"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
+
+    <define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
+  </section>
+
+  <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
+    <define name="COURSE_PGAIN" value="1.0"/>
+    <define name="COURSE_DGAIN" value="0.3"/>
+
+    <define name="ROLL_MAX_SETPOINT"  value="35"  unit="deg"/>
+    <define name="PITCH_MAX_SETPOINT" value="30"  unit="deg"/>
+    <define name="PITCH_MIN_SETPOINT" value="-30" unit="deg"/>
+
+    <define name="PITCH_PGAIN" value="12000."/>
+    <define name="PITCH_DGAIN" value="1.5"/>
+
+    <define name="ELEVATOR_OF_ROLL" value="1250"/>
+
+    <define name="ROLL_SLEW" value="0.1"/>
+
+    <define name="ROLL_ATTITUDE_GAIN" value="7500"/>
+    <define name="ROLL_RATE_GAIN" value="1500"/>
+  </section>
+
+  <section name="AGGRESSIVE" prefix="AGR_">
+    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
+    <define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
+    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
+    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="DESCENT_NAV_RATIO" value="1.0"/>
+    </section>
+
+  <section name="FAILSAFE" prefix="FAILSAFE_">
+    <define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
+    <define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
+    <define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
+    <define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
+  </section>
+
+  <section name="AUTO1" prefix="AUTO1_">
+    <define name="MAX_ROLL" value="40." unit="deg"/>
+    <define name="MAX_PITCH" value="35." unit="deg"/>
+  </section>
+
+  <section name="BAT">
+    <define name="MILLIAMP_AT_FULL_THROTTLE" value="2000"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
+    <define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
+    <define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
+  </section>
+
+  <section name="MISC">
+    <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
+    <define name="CARROT" value="5." unit="s"/>
+    <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
+    <define name="CONTROL_RATE" value="60" unit="Hz"/>
+    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+    <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
+  </section>
+
+</airframe>
diff --git a/conf/airframes/flixr_discovery.xml b/conf/airframes/flixr_discovery.xml
index ef597b4a59..23adaad4d4 100644
--- a/conf/airframes/flixr_discovery.xml
+++ b/conf/airframes/flixr_discovery.xml
@@ -13,7 +13,6 @@
       <configure name="AHRS_PROPAGATE_FREQUENCY" value="120"/>
       <configure name="AHRS_CORRECT_FREQUENCY" value="60"/>
       <configure name="AHRS_ALIGNER_LED" value="2"/>
-      <!--configure name="CPU_LED" value="2"/-->
       <configure name="SYS_TIME_LED" value="1"/>
       <configure name="GPS_LED" value="none"/>
       <!-- for airspeed sensor
diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile
index fe4b04a0ee..776a1f313f 100644
--- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile
+++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile
@@ -34,10 +34,6 @@ ap.srcs += $(AHRS_SRCS)
 
 # Extra stuff for fixedwings
 
-ifdef CPU_LED
-  ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
-endif
-
 ifdef AHRS_PROPAGATE_FREQUENCY
 else
   AHRS_PROPAGATE_FREQUENCY = 60
diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile
index 50c352f4eb..b9a40ea5dd 100644
--- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile
+++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile
@@ -34,10 +34,6 @@ ap.srcs += $(AHRS_SRCS)
 
 # Extra stuff for fixedwings
 
-ifdef CPU_LED
-  ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
-endif
-
 ifdef AHRS_PROPAGATE_FREQUENCY
 else
   AHRS_PROPAGATE_FREQUENCY = 60
diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile
index bb22a1105d..0c38464a2b 100644
--- a/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile
+++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile
@@ -25,10 +25,6 @@ ifneq ($(AHRS_ALIGNER_LED),none)
   ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
 
-ifdef CPU_LED
-  ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
-endif
-
 ifdef AHRS_PROPAGATE_FREQUENCY
 else
   AHRS_PROPAGATE_FREQUENCY = 60
diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile
index a9c10259b0..708d859b25 100644
--- a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile
+++ b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile
@@ -30,10 +30,6 @@ ap.srcs += $(AHRS_SRCS)
 
 # Extra stuff for fixedwings
 
-ifdef CPU_LED
-  ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
-endif
-
 ifdef AHRS_PROPAGATE_FREQUENCY
 else
   AHRS_PROPAGATE_FREQUENCY = 60
diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile
index 62189bbc32..0849421c28 100644
--- a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile
+++ b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile
@@ -33,10 +33,6 @@ ap.srcs += $(AHRS_SRCS)
 
 # Extra stuff for fixedwings
 
-ifdef CPU_LED
-  ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
-endif
-
 ifndef AHRS_PROPAGATE_FREQUENCY
   AHRS_PROPAGATE_FREQUENCY = 60
 endif
diff --git a/conf/firmwares/subsystems/fixedwing/imu_xsens.makefile b/conf/firmwares/subsystems/fixedwing/imu_xsens.makefile
new file mode 100644
index 0000000000..48fb6d7eec
--- /dev/null
+++ b/conf/firmwares/subsystems/fixedwing/imu_xsens.makefile
@@ -0,0 +1,68 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+# XSens Mti just providing IMU measurements
+
+#    <subsystem name="imu" type="xsens">
+#      <configure name="XSENS_UART_NR" value="0"/>
+#      <configure name="XSENS_UART_BAUD" value="B115200"/>
+#    </subsystem>
+#
+#  <section name="IMU" prefix="IMU_">
+#    <define name="GYRO_P_SIGN"  value="1"/>
+#    <define name="GYRO_Q_SIGN"  value="1"/>
+#    <define name="GYRO_R_SIGN"  value="1"/>
+#
+#    <define name="GYRO_P_NEUTRAL" value="0"/>
+#    <define name="GYRO_R_NEUTRAL" value="0"/>
+#    <define name="GYRO_Q_NEUTRAL" value="0"/>
+#
+#    <define name="GYRO_P_SENS" value="1" integer="16"/>
+#    <define name="GYRO_R_SENS" value="1" integer="16"/>
+#    <define name="GYRO_Q_SENS" value="1" integer="16"/>
+#
+#    <define name="ACCEL_X_SIGN"  value="1"/>
+#    <define name="ACCEL_Y_SIGN"  value="1"/>
+#    <define name="ACCEL_Z_SIGN"  value="1"/>
+#
+#    <define name="ACCEL_X_SENS" value="1" integer="16"/>
+#    <define name="ACCEL_Z_SENS" value="1" integer="16"/>
+#    <define name="ACCEL_Y_SENS" value="1" integer="16"/>
+#
+#    <define name="ACCEL_X_NEUTRAL" value="0"/>
+#    <define name="ACCEL_Z_NEUTRAL" value="0"/>
+#    <define name="ACCEL_Y_NEUTRAL" value="0"/>
+#
+#    <define name="MAG_X_SIGN" value="1"/>
+#    <define name="MAG_Y_SIGN" value="1"/>
+#    <define name="MAG_Z_SIGN" value="1"/>
+#
+#    <define name="MAG_X_NEUTRAL" value="-45"/>
+#    <define name="MAG_Y_NEUTRAL" value="334"/>
+#    <define name="MAG_Z_NEUTRAL" value="7"/>
+#
+#    <define name="MAG_X_SENS" value="4.47647816128" integer="16"/>
+#    <define name="MAG_Y_SENS" value="4.71085671542" integer="16"/>
+#    <define name="MAG_Z_SENS" value="4.41585354498" integer="16"/>
+#
+#    <define name="BODY_TO_IMU_PHI" value="0" unit="deg"/>
+#    <define name="BODY_TO_IMU_THETA" value="0" unit="deg"/>
+#    <define name="BODY_TO_IMU_PSI" value="0" unit="deg"/>
+#  </section>
+
+
+#########################################
+## IMU
+
+ap.CFLAGS += -DUSE_IMU
+ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_xsens.h\"
+ap.srcs   += $(SRC_MODULES)/ins/ins_xsens.c
+ap.srcs   += $(SRC_SUBSYSTEMS)/imu.c
+
+ifndef XSENS_UART_BAUD
+	XSENS_UART_BAUD = B115200
+endif
+
+ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR)
+ap.CFLAGS += -DINS_LINK=Uart$(XSENS_UART_NR)
+ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD)
+ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836
diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
index 85ab233c38..599d470f91 100644
--- a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
+++ b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
@@ -2,9 +2,10 @@
 
 # XSens Mti-G
 
-#    <load name="ins_xsens_MTiG_fixedwing.xml">
+#    <subsystem name="ins" type="xsens">
 #      <configure name="XSENS_UART_NR" value="0"/>
-#    </load>
+#      <configure name="XSENS_UART_BAUD" value="B115200"/>
+#    </subsystem>
 
 
 
@@ -13,41 +14,21 @@
 
 ifeq ($(TARGET), ap)
 
-#  <init fun="ins_init()"/>
-#  <periodic fun="ins_periodic_task()" freq="60"/>
-#  <event fun="InsEventCheckAndHandle(handle_ins_msg())"/>
-#  <makefile target="ap">
-#    <define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_xsens.h\\\"" />
-#    <define name="INS_MODULE_H" value="\\\"modules/ins/ins_xsens.h\\\"" />
-#    <define name="USE_UART$(XSENS_UART_NR)"/>
-#    <define name="INS_LINK" value="Uart$(XSENS_UART_NR)"/>
-#    <define name="UART$(XSENS_UART_NR)_BAUD" value="B230400"/>
-#    <define name="USE_GPS_XSENS"/>
-#    <define name="USE_GPS_XSENS_RAW_DATA" />
-#    <define name="GPS_NB_CHANNELS" value="16" />
-#    <define name="XSENS_OUTPUT_MODE" value="0x1836" />
-#    <file name="ins_xsens.c"/>
-#    <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
-#  </makefile>
-
-# ImuEvent -> XSensEvent
-ap.CFLAGS += -DUSE_AHRS -DUSE_INS
-ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_xsens.h\"
+ap.CFLAGS += -DUSE_INS
 
 # AHRS Results
 ap.CFLAGS += -DINS_MODULE_H=\"modules/ins/ins_xsens.h\"
-ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_xsens.h\"
-ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\"
+
+ifndef XSENS_UART_BAUD
+	XSENS_UART_BAUD = B115200
+endif
 
 #B230400
 #B115200
 
 ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR)
 ap.CFLAGS += -DINS_LINK=Uart$(XSENS_UART_NR)
-ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=B230400
-ap.CFLAGS += -DUSE_GPS_XSENS
-ap.CFLAGS += -DUSE_GPS_XSENS_RAW_DATA
-ap.CFLAGS += -DGPS_NB_CHANNELS=16
+ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD)
 ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836
 ap.srcs   += $(SRC_MODULES)/ins/ins_xsens.c
 ap.CFLAGS += -DAHRS_TRIGGERED_ATTITUDE_LOOP
@@ -69,13 +50,17 @@ endif
 #########################################
 ## GPS
 
-# ap.CFLAGS += -DGPS
-
-$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+ap.CFLAGS += -DUSE_GPS_XSENS
+ap.CFLAGS += -DUSE_GPS_XSENS_RAW_DATA
+ap.CFLAGS += -DGPS_NB_CHANNELS=16
+ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
+ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\"
+ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
 
 sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
 sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
 sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
+sim.srcs += $(SRC_SUBSYSTEMS)/gps.c
 
 
 
diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h
index 0953cad086..90311b1acb 100644
--- a/sw/airborne/firmwares/fixedwing/ap_downlink.h
+++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h
@@ -207,8 +207,10 @@
 
 #define PERIODIC_SEND_TUNE_ROLL(_trans, _dev) DOWNLINK_SEND_TUNE_ROLL(_trans, _dev, &estimator_p,&estimator_phi, &h_ctl_roll_setpoint);
 
-#if USE_GPS || USE_GPS_XSENS || defined SITL
+#if USE_GPS || defined SITL
 #define PERIODIC_SEND_GPS_SOL(_trans, _dev) DOWNLINK_SEND_GPS_SOL(_trans, _dev, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv)
+#else
+#define PERIODIC_SEND_GPS_SOL(_trans, _dev) {}
 #endif
 
 #define PERIODIC_SEND_GPS(_trans, _dev) {                                      \
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index dc15384bec..e1231a973f 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -93,8 +93,6 @@ static inline void on_gyro_event( void );
 static inline void on_accel_event( void );
 static inline void on_mag_event( void );
 volatile uint8_t ahrs_timeout_counter = 0;
-#else
-static inline void on_ahrs_event(void);
 #endif // USE_IMU
 #endif // USE_AHRS
 
@@ -174,6 +172,10 @@ void init_ap( void ) {
   ahrs_init();
 #endif
 
+#if USE_INS
+  ins_init();
+#endif
+
   /************* Links initialization ***************/
 #if defined MCU_SPI_LINK
   link_mcu_init();
@@ -479,11 +481,9 @@ void navigation_task( void ) {
 }
 
 
-#if USE_AHRS
 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
 volatile uint8_t new_ins_attitude = 0;
 #endif
-#endif
 
 void attitude_loop( void ) {
 
@@ -518,6 +518,10 @@ void sensors_task( void ) {
     ahrs_timeout_counter ++;
 #endif // USE_AHRS
 #endif // USE_IMU
+
+#if USE_INS
+  ins_periodic_task();
+#endif
 }
 
 
@@ -571,17 +575,17 @@ void event_task_ap( void ) {
   i2c_event();
 #endif
 
-#if USE_AHRS
-#if USE_IMU
+#if USE_AHRS && USE_IMU
   ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
-#else
-  AhrsEvent(on_ahrs_event);
-#endif // USE_IMU
-#endif // USE_AHRS
+#endif
+
+#if USE_INS
+  InsEvent(NULL);
+#endif
 
 #if USE_GPS
   GpsEvent(on_gps_solution);
-#endif /** USE_GPS */
+#endif /* USE_GPS */
 
 
   DatalinkEvent();
@@ -614,7 +618,9 @@ void event_task_ap( void ) {
 #if USE_GPS
 static inline void on_gps_solution( void ) {
   estimator_update_state_gps();
+#if USE_AHRS
   ahrs_update_gps();
+#endif
 #ifdef GPS_TRIGGERED_FUNCTION
   GPS_TRIGGERED_FUNCTION();
 #endif
@@ -631,10 +637,6 @@ static inline void on_gyro_event( void ) {
 
   ahrs_timeout_counter = 0;
 
-#ifdef AHRS_CPU_LED
-  LED_ON(AHRS_CPU_LED);
-#endif
-
 #if USE_AHRS_ALIGNER
   // Run aligner on raw data as it also makes averages.
   if (ahrs.status == AHRS_UNINIT) {
@@ -701,10 +703,6 @@ static inline void on_gyro_event( void ) {
   }
 #endif //PERIODIC_FREQUENCY
 
-#ifdef AHRS_CPU_LED
-    LED_OFF(AHRS_CPU_LED);
-#endif
-
 }
 
 static inline void on_mag_event(void)
@@ -717,13 +715,6 @@ static inline void on_mag_event(void)
 #endif
 }
 
-#else // USE_IMU not defined
-static inline void on_ahrs_event(void)
-{
-#ifdef AHRS_UPDATE_FW_ESTIMATOR
-  ahrs_update_fw_estimator();
-#endif
-}
 #endif // USE_IMU
 
 #endif // USE_AHRS
diff --git a/sw/airborne/modules/ins/ins_module.h b/sw/airborne/modules/ins/ins_module.h
index 194dcb5f89..1bab2b1181 100644
--- a/sw/airborne/modules/ins/ins_module.h
+++ b/sw/airborne/modules/ins/ins_module.h
@@ -32,7 +32,6 @@
 #define INS_H
 
 #include "std.h"
-#include "led.h"
 
 #ifndef INS_FORMAT
 #define INS_FORMAT float
@@ -62,9 +61,10 @@ extern INS_FORMAT ins_mx;
 extern INS_FORMAT ins_my;
 extern INS_FORMAT ins_mz;
 
+#if USE_INS
 extern INS_FORMAT ins_roll_neutral;
 extern INS_FORMAT ins_pitch_neutral;
-
+#endif
 
 extern volatile uint8_t ins_msg_received;
 extern volatile uint8_t new_ins_attitude;
@@ -97,7 +97,6 @@ void parse_ins_buffer( uint8_t );
       ReadInsBuffer();						\
     }						                \
     if (ins_msg_received) {					\
-      LED_TOGGLE(2);						\
       parse_ins_msg();						\
       handler;							\
       ins_msg_received = FALSE;					\
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index be4e36b6b6..bfe3790d9d 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -1,6 +1,4 @@
 /*
- * Paparazzi mcu0 $Id$
- *
  * Copyright (C) 2003  Pascal Brisset, Antoine Drouin
  *
  * This file is part of paparazzi.
@@ -22,8 +20,8 @@
  *
  */
 
-/** \file xsens.c
- * \brief Parser for the Xsens protocol
+/** @file xsens.c
+ * Parser for the Xsens protocol.
  */
 
 #include "ins_module.h"
@@ -38,10 +36,14 @@
 #include "messages.h"
 
 #if USE_GPS_XSENS
+#if !USE_GPS
+#error "USE_GPS needs to be 1 to use the Xsens GPS!"
+#endif
 #include "subsystems/gps.h"
 #include "math/pprz_geodetic_wgs84.h"
 #include "math/pprz_geodetic_float.h"
 #include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */
+bool_t gps_xsens_msg_available;
 #endif
 
 INS_FORMAT ins_x;
@@ -68,32 +70,12 @@ INS_FORMAT ins_mx;
 INS_FORMAT ins_my;
 INS_FORMAT ins_mz;
 
+#if USE_INS
 float ins_pitch_neutral;
 float ins_roll_neutral;
-
-
-void ahrs_init(void)
-{
-  ins_init();
-}
-
-
-#ifdef USE_IMU
-
-#include "subsystems/imu.h"
-
-void imu_init(void)
-{
-  ins_init();
-}
-
-void imu_periodic(void)
-{
-  ins_periodic_task();
-}
-
 #endif
 
+
 //////////////////////////////////////////////////////////////////////////////////////////
 //
 //	XSens Specific
@@ -201,19 +183,19 @@ uint8_t xsens_msg_status;
 uint16_t xsens_time_stamp;
 uint16_t xsens_output_mode;
 uint32_t xsens_output_settings;
+
+
 float xsens_declination = 0;
 float xsens_gps_arm_x = 0;
 float xsens_gps_arm_y = 0;
 float xsens_gps_arm_z = 0;
 
+#if USE_GPS_XSENS
+struct LlaCoor_f lla_f;
+struct UtmCoor_f utm_f;
+#endif
 
-int8_t xsens_hour;
-int8_t xsens_min;
-int8_t xsens_sec;
-int32_t xsens_nanosec;
-int16_t xsens_year;
-int8_t xsens_month;
-int8_t xsens_day;
+struct XsensTime xsens_time;
 
 static uint8_t xsens_id;
 static uint8_t xsens_status;
@@ -222,9 +204,6 @@ static uint8_t xsens_msg_idx;
 static uint8_t ck;
 uint8_t send_ck;
 
-struct LlaCoor_f lla_f;
-struct UtmCoor_f utm_f;
-
 volatile int xsens_configured = 0;
 
 void ins_init( void ) {
@@ -232,80 +211,104 @@ void ins_init( void ) {
   xsens_status = UNINIT;
   xsens_configured = 20;
 
+#if USE_INS
   ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
   ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+#endif
 
   xsens_msg_status = 0;
   xsens_time_stamp = 0;
   xsens_output_mode = XSENS_OUTPUT_MODE;
   xsens_output_settings = XSENS_OUTPUT_SETTINGS;
-
-  gps.nb_channels = 0;
 }
 
+#if USE_IMU
+struct ImuXsens imu_xsens;
+
+void imu_impl_init(void) {
+  ins_init();
+  imu_xsens.gyro_available = FALSE;
+  imu_xsens.accel_available = FALSE;
+  imu_xsens.mag_available = FALSE;
+}
+
+void imu_periodic(void) {
+  ins_periodic_task();
+}
+#endif /* USE_IMU */
+
+#if USE_GPS_XSENS
+void gps_impl_init(void) {
+  gps.nb_channels = 0;
+  gps_xsens_msg_available = FALSE;
+}
+#endif
+
 void ins_periodic_task( void ) {
   if (xsens_configured > 0)
-  {
-     switch (xsens_configured)
-     {
-	case 20:
-		/* send mode and settings to MT */
-		XSENS_GoToConfig();
-		XSENS_SetOutputMode(xsens_output_mode);
-		XSENS_SetOutputSettings(xsens_output_settings);
-		break;
-	case 18:
-		// Give pulses on SyncOut
-		XSENS_SetSyncOutSettings(0,0x0002);
-		break;
-	case 17:
-		// 1 pulse every 100 samples
-		XSENS_SetSyncOutSettings(1,100);
-		break;
-	case 2:
-		XSENS_ReqLeverArmGps();
-		break;
+    {
+      switch (xsens_configured)
+        {
+        case 20:
+          /* send mode and settings to MT */
+          XSENS_GoToConfig();
+          XSENS_SetOutputMode(xsens_output_mode);
+          XSENS_SetOutputSettings(xsens_output_settings);
+          break;
+        case 18:
+          // Give pulses on SyncOut
+          XSENS_SetSyncOutSettings(0,0x0002);
+          break;
+        case 17:
+          // 1 pulse every 100 samples
+          XSENS_SetSyncOutSettings(1,100);
+          break;
+        case 2:
+          XSENS_ReqLeverArmGps();
+          break;
 
-	case 6:
-		XSENS_ReqMagneticDeclination();
-		break;
+        case 6:
+          XSENS_ReqMagneticDeclination();
+          break;
 
-	case 13:
-		#ifdef AHRS_H_X
-		#pragma message "Sending XSens Magnetic Declination."
-		xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
-		XSENS_SetMagneticDeclination(xsens_declination);
-		#endif
-		break;
-	case 12:
-		#ifdef GPS_IMU_LEVER_ARM_X
-		#pragma message "Sending XSens GPS Arm."
-		XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z);
-		#endif
-		break;
-	case 10:
-		{
-		uint8_t baud = 1;
-		XSENS_SetBaudrate(baud);
-		}
-		break;
+        case 13:
+#ifdef AHRS_H_X
+#pragma message "Sending XSens Magnetic Declination."
+          xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
+          XSENS_SetMagneticDeclination(xsens_declination);
+#endif
+          break;
+        case 12:
+#ifdef GPS_IMU_LEVER_ARM_X
+#pragma message "Sending XSens GPS Arm."
+          XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z);
+#endif
+          break;
+        case 10:
+          {
+            uint8_t baud = 1;
+            XSENS_SetBaudrate(baud);
+          }
+          break;
 
-	case 1:
-		XSENS_GoToMeasurment();
-		break;
-     }
-     xsens_configured--;
-     return;
-  }
+        case 1:
+          XSENS_GoToMeasurment();
+          break;
+
+        default:
+          break;
+        }
+      xsens_configured--;
+      return;
+    }
 
   RunOnceEvery(100,XSENS_ReqGPSStatus());
 }
 
+#if USE_INS
 #include "estimator.h"
 
-void handle_ins_msg( void) {
-
-
+static inline void update_fw_estimator(void) {
   // Send to Estimator (Control)
 #ifdef XSENS_BACKWARDS
   EstimatorSetAtt((-ins_phi+ins_roll_neutral), (ins_psi + RadOfDeg(180)), (-ins_theta+ins_pitch_neutral));
@@ -314,38 +317,55 @@ void handle_ins_msg( void) {
   EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral);
   EstimatorSetRate(ins_p, ins_q, ins_r);
 #endif
+}
+#endif /* USE_INS */
 
-  // Position
-  float gps_east = gps.utm_pos.east / 100.;
-  float gps_north = gps.utm_pos.north / 100.;
-  gps_east -= nav_utm_east0;
-  gps_north -= nav_utm_north0;
-  EstimatorSetPosXY(gps_east, gps_north);
+void handle_ins_msg(void) {
 
-  // Altitude and vertical speed
-  float hmsl = gps.hmsl;
-  hmsl /= 1000.0f;
-  EstimatorSetAlt(hmsl);
+#if USE_INS
+  update_fw_estimator();
+#endif
 
+#if USE_IMU
+#ifdef XSENS_BACKWARDS
+  if (imu_xsens.gyro_available) {
+    RATES_ASSIGN(imu.gyro_unscaled, -RATE_BFP_OF_REAL(ins_p), -RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
+  }
+  if (imu_xsens.accel_available) {
+    VECT3_ASSIGN(imu.accel_unscaled, -ACCEL_BFP_OF_REAL(ins_ax), -ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
+  }
+  if (imu_xsens.mag_available) {
+    VECT3_ASSIGN(imu.mag_unscaled, -MAG_BFP_OF_REAL(ins_mx), -MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
+  }
+#else
+  if (imu_xsens.gyro_available) {
+    RATES_ASSIGN(imu.gyro_unscaled, RATE_BFP_OF_REAL(ins_p), RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
+  }
+  if (imu_xsens.accel_available) {
+    VECT3_ASSIGN(imu.accel_unscaled, ACCEL_BFP_OF_REAL(ins_ax), ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
+  }
+  if (imu_xsens.mag_available) {
+    VECT3_ASSIGN(imu.mag_unscaled, MAG_BFP_OF_REAL(ins_mx), MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
+  }
+#endif /* XSENS_BACKWARDS */
+#endif /* USE_IMU */
+
+#if USE_GPS_XSENS
   #ifndef ALT_KALMAN
   #warning NO_VZ
   #endif
 
   // Horizontal speed
   float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
-  if (gps.fix != GPS_FIX_3D)
-  {
+  if (gps.fix != GPS_FIX_3D) {
     fspeed = 0;
   }
-  float fclimb = -ins_vz;
-  float fcourse = atan2f((float)ins_vy, (float)ins_vx);
-  EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
-
-  // Now also finish filling the gps struct for telemetry purposes
   gps.gspeed = fspeed * 100.;
   gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
-  gps.course = fcourse * 1e7;
 
+  float fcourse = atan2f((float)ins_vy, (float)ins_vx);
+  gps.course = fcourse * 1e7;
+#endif // USE_GPS_XSENS
 }
 
 void parse_ins_msg( void ) {
@@ -376,8 +396,6 @@ void parse_ins_msg( void ) {
     gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
     gps.num_sv = 0;
 
-    gps.last_fix_time = sys_time.nb_sec;
-
     uint8_t i;
     // Do not write outside buffer
     for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
@@ -401,21 +419,20 @@ void parse_ins_msg( void ) {
         ins_p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf,offset);
         ins_q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf,offset);
         ins_r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf,offset);
+#if USE_IMU
+        imu_xsens.gyro_available = TRUE;
+#endif
         offset += XSENS_DATA_RAWInertial_LENGTH;
       }
       if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
 #if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS
-#ifdef GPS_LED
-    LED_TOGGLE(GPS_LED);
-#endif
-        gps.last_fix_time = sys_time.nb_sec;
+
         gps.week = 0; // FIXME
         gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
         gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
         gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset));
         gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset);
 
-
         /* Set the real UTM zone */
         gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
 
@@ -434,9 +451,9 @@ void parse_ins_msg( void ) {
         // Altitude: Xsens LLH gives ellipsoid height
         ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.;
 
-	// Compute geoid (MSL) height
+        // Compute geoid (MSL) height
         float hmsl;
-	WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl);
+        WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl);
         gps.hmsl =  XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f);
 
         ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset)) / 100.;
@@ -448,6 +465,8 @@ void parse_ins_msg( void ) {
         gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
         gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
         gps.pdop = 5;  // FIXME Not output by XSens
+
+        gps_xsens_msg_available = TRUE;
 #endif
         offset += XSENS_DATA_RAWGPS_LENGTH;
       }
@@ -461,18 +480,27 @@ void parse_ins_msg( void ) {
           ins_ax = XSENS_DATA_Calibrated_accX(xsens_msg_buf,offset);
           ins_ay = XSENS_DATA_Calibrated_accY(xsens_msg_buf,offset);
           ins_az = XSENS_DATA_Calibrated_accZ(xsens_msg_buf,offset);
+#if USE_IMU
+          imu_xsens.accel_available = TRUE;
+#endif
           l++;
         }
         if (!XSENS_MASK_GyrOut(xsens_output_settings)) {
           ins_p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf,offset);
           ins_q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf,offset);
           ins_r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf,offset);
+#if USE_IMU
+          imu_xsens.gyro_available = TRUE;
+#endif
           l++;
         }
         if (!XSENS_MASK_MagOut(xsens_output_settings)) {
           ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf,offset);
           ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf,offset);
           ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf,offset);
+#if USE_IMU
+          imu_xsens.mag_available = TRUE;
+#endif
           l++;
         }
         offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
@@ -516,8 +544,6 @@ void parse_ins_msg( void ) {
       }
       if (XSENS_MASK_Position(xsens_output_mode)) {
 #if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
-        gps.last_fix_time = sys_time.nb_sec;
-
         lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset));
         lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset));
         gps.lla_pos.lat = (int32_t)(lla_f.lat * 1e7);
@@ -533,6 +559,8 @@ void parse_ins_msg( void ) {
         ins_z = XSENS_DATA_Position_alt(xsens_msg_buf,offset);//TODO is this hms or above ellipsoid?
         gps.hmsl = ins_z * 1000;
         // what about gps.lla_pos.alt and gps.utm_pos.alt ?
+
+        gps_xsens_msg_available = TRUE;
 #endif
         offset += XSENS_DATA_Position_LENGTH;
       }
@@ -550,7 +578,15 @@ void parse_ins_msg( void ) {
         if (bit_is_set(xsens_msg_status,2)) gps.fix = GPS_FIX_3D; // gps fix
         else if (bit_is_set(xsens_msg_status,1)) gps.fix = 0x01; // efk valid
         else gps.fix = GPS_FIX_NONE;
-#endif
+#ifdef GPS_LED
+        if (gps.fix == GPS_FIX_3D) {
+          LED_ON(GPS_LED);
+        }
+        else {
+          LED_TOGGLE(GPS_LED);
+        }
+#endif // GPS_LED
+#endif //  USE_GPS_XSENS
         offset += XSENS_DATA_Status_LENGTH;
       }
       if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
@@ -561,13 +597,13 @@ void parse_ins_msg( void ) {
         offset += XSENS_DATA_TimeStamp_LENGTH;
       }
       if (XSENS_MASK_UTC(xsens_output_settings)) {
-        xsens_hour = XSENS_DATA_UTC_hour(xsens_msg_buf,offset);
-        xsens_min = XSENS_DATA_UTC_min(xsens_msg_buf,offset);
-        xsens_sec = XSENS_DATA_UTC_sec(xsens_msg_buf,offset);
-        xsens_nanosec = XSENS_DATA_UTC_nanosec(xsens_msg_buf,offset);
-        xsens_year = XSENS_DATA_UTC_year(xsens_msg_buf,offset);
-        xsens_month = XSENS_DATA_UTC_month(xsens_msg_buf,offset);
-        xsens_day = XSENS_DATA_UTC_day(xsens_msg_buf,offset);
+        xsens_time.hour = XSENS_DATA_UTC_hour(xsens_msg_buf,offset);
+        xsens_time.min = XSENS_DATA_UTC_min(xsens_msg_buf,offset);
+        xsens_time.sec = XSENS_DATA_UTC_sec(xsens_msg_buf,offset);
+        xsens_time.nanosec = XSENS_DATA_UTC_nanosec(xsens_msg_buf,offset);
+        xsens_time.year = XSENS_DATA_UTC_year(xsens_msg_buf,offset);
+        xsens_time.month = XSENS_DATA_UTC_month(xsens_msg_buf,offset);
+        xsens_time.day = XSENS_DATA_UTC_day(xsens_msg_buf,offset);
 
         offset += XSENS_DATA_UTC_LENGTH;
       }
@@ -614,6 +650,8 @@ void parse_ins_buffer( uint8_t c ) {
     ins_msg_received = TRUE;
     goto restart;
     break;
+  default:
+    break;
   }
   return;
  error:
diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h
index a042d67d12..595336315e 100644
--- a/sw/airborne/modules/ins/ins_xsens.h
+++ b/sw/airborne/modules/ins/ins_xsens.h
@@ -33,23 +33,72 @@
 
 #include "ins_module.h"
 
-#include "subsystems/ahrs.h"
+struct XsensTime {
+  int8_t hour;
+  int8_t min;
+  int8_t sec;
+  int32_t nanosec;
+  int16_t year;
+  int8_t month;
+  int8_t day;
+};
 
+extern struct XsensTime xsens_time;
 
-extern int8_t xsens_hour;
-extern int8_t xsens_min;
-extern int8_t xsens_sec;
-extern int32_t xsens_nanosec;
-extern int16_t xsens_year;
-extern int8_t xsens_month;
-extern int8_t xsens_day;
 extern uint8_t xsens_msg_status;
 extern uint16_t xsens_time_stamp;
 
-#define AhrsEvent(_ahrs_handler) {	\
-  LED_TOGGLE(3); \
+
+/* To use Xsens to just provide IMU measurements
+ * for use with an external AHRS algorithm
+ */
+#if USE_IMU
+#include "subsystems/imu.h"
+
+struct ImuXsens {
+  bool_t gyro_available;
+  bool_t accel_available;
+  bool_t mag_available;
+};
+extern struct ImuXsens imu_xsens;
+
+#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
+    if (imu_xsens.accel_available) {                            \
+      imu_xsens.accel_available = FALSE;                        \
+      _accel_handler();                                         \
+    }                                                           \
+    if (imu_xsens.gyro_available) {                             \
+      imu_xsens.gyro_available = FALSE;                         \
+      _gyro_handler();                                          \
+    }                                                           \
+    if (imu_xsens.mag_available) {                              \
+      imu_xsens.mag_available = FALSE;                          \
+      _mag_handler();                                           \
+    }                                                           \
+  }
+#endif /* USE_IMU */
+
+
+/* use Xsens as a full INS solution */
+#if USE_INS
+#define InsEvent(_ins_handler) {	\
   InsEventCheckAndHandle(handle_ins_msg()) 			\
 }
+#endif
 
 
+#if USE_GPS_XSENS
+extern bool_t gps_xsens_msg_available;
+#define GpsEvent(_sol_available_callback) {         \
+    if (gps_xsens_msg_available) {                  \
+      if (gps.fix == GPS_FIX_3D) {                  \
+        gps.last_fix_ticks = sys_time.nb_sec_rem;   \
+        gps.last_fix_time = sys_time.nb_sec;        \
+      }                                             \
+      _sol_available_callback();                    \
+      gps_xsens_msg_available = FALSE;              \
+    }                                               \
+  }
+#endif
+
 #endif
diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.h b/sw/airborne/subsystems/ahrs/ahrs_infrared.h
index 43a2b3b340..a4d2870756 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_infrared.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.h
@@ -36,7 +36,4 @@ extern void ahrs_update_infrared(void);
 // TODO copy ahrs to state instead of estimator
 extern void ahrs_update_fw_estimator(void);
 
-#define AhrsEvent(_available_callback) {        \
-  }
-
 #endif /* AHRS_INFRARED_H */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.c b/sw/airborne/subsystems/ahrs/ahrs_sim.c
index 290602091b..3dcfcf0263 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_sim.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_sim.c
@@ -53,6 +53,10 @@ void update_ahrs_from_sim(void) {
   FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler);
 
   compute_body_orientation_and_rates();
+
+#ifdef AHRS_UPDATE_FW_ESTIMATOR
+  ahrs_update_fw_estimator();
+#endif
 }
 
 
@@ -92,6 +96,10 @@ void ahrs_align(void)
 
 
 void ahrs_propagate(void) {
+  if (ahrs_sim_available) {
+    update_ahrs_from_sim();
+    ahrs_sim_available = FALSE;
+  }
 }
 
 void ahrs_update_accel(void) {
diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.h b/sw/airborne/subsystems/ahrs/ahrs_sim.h
index 7aca2313a7..ee36d402be 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_sim.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_sim.h
@@ -37,14 +37,4 @@ extern float ins_pitch_neutral;
 
 extern void update_ahrs_from_sim(void);
 
-#define AhrsEvent(_available_callback) {        \
-    if (ahrs_sim_available) {                   \
-      update_ahrs_from_sim();               \
-      _available_callback();                    \
-      ahrs_sim_available = FALSE;               \
-    }                                           \
-  }
-
-
-
 #endif /* AHRS_SIM_H */