diff --git a/conf/modules/imu_xsens.xml b/conf/modules/imu_xsens.xml
index 29312340a7..4078bbadb0 100644
--- a/conf/modules/imu_xsens.xml
+++ b/conf/modules/imu_xsens.xml
@@ -19,7 +19,7 @@
   <event fun="imu_xsens_event()"/>
   <makefile target="ap">
     <file name="xsens.c"/>
-    <file name="xsens_common.c"/>
+    <file name="xsens_parser.c"/>
     <file name="imu_xsens.c"/>
 
     <configure name="XSENS_PORT" default="uart1" case="upper|lower"/>
diff --git a/conf/modules/ins_xsens.xml b/conf/modules/ins_xsens.xml
index 115a55ac5f..c97e6f61f4 100644
--- a/conf/modules/ins_xsens.xml
+++ b/conf/modules/ins_xsens.xml
@@ -20,7 +20,7 @@
   <event fun="ins_xsens_event()"/>
   <makefile target="ap">
     <file name="xsens.c"/>
-    <file name="xsens_common.c"/>
+    <file name="xsens_parser.c"/>
     <file name="ins_xsens.c"/>
     <file name="ins.c" dir="subsystems"/>
     <configure name="XSENS_PORT" default="uart1" case="upper|lower"/>
diff --git a/conf/modules/ins_xsens700.xml b/conf/modules/ins_xsens700.xml
index 4b2621b4d2..77746fef3c 100644
--- a/conf/modules/ins_xsens700.xml
+++ b/conf/modules/ins_xsens700.xml
@@ -20,7 +20,7 @@
   <event fun="ins_xsens700_event()"/>
   <makefile target="ap">
     <file name="xsens700.c"/>
-    <file name="xsens_common.c"/>
+    <file name="xsens_parser.c"/>
     <file name="ins_xsens700.c"/>
     <file name="ins.c" dir="subsystems"/>
     <configure name="XSENS_PORT" default="uart1" case="upper|lower"/>
diff --git a/sw/airborne/modules/ins/imu_xsens.c b/sw/airborne/modules/ins/imu_xsens.c
index 37c186dd8f..46e0bbd24b 100644
--- a/sw/airborne/modules/ins/imu_xsens.c
+++ b/sw/airborne/modules/ins/imu_xsens.c
@@ -27,7 +27,6 @@
 
 #include "imu_xsens.h"
 #include "xsens.h"
-#include "xsens_common.h"
 
 #include "generated/airframe.h"
 
@@ -43,11 +42,11 @@ void imu_xsens_init(void)
 
 void imu_xsens_event(void)
 {
-  xsens_event();
-  if (xsens.msg_received) {
+  xsens_parser_event(&(xsens.parser));
+  if (xsens.parser.msg_received) {
     parse_xsens_msg();
     handle_ins_msg();
-    xsens.msg_received = FALSE;
+    xsens.parser.msg_received = FALSE;
   }
 }
 
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index 6baede2328..09e252c55a 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -25,7 +25,6 @@
  */
 
 #include "ins_xsens.h"
-#include "xsens_common.h"
 #include "subsystems/ins.h"
 
 #include "generated/airframe.h"
@@ -77,11 +76,11 @@ void ins_xsens_init(void)
 
 void ins_xsens_event(void)
 {
-  xsens_event();
-  if (xsens.msg_received) {
+  xsens_parser_event(&(xsens.parser));
+  if (xsens.parser.msg_received) {
     parse_xsens_msg();
     handle_ins_msg();
-    xsens.msg_received = false;
+    xsens.parser.msg_received = FALSE;
   }
 }
 
diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c
index 38eaeac523..cebd3801b1 100644
--- a/sw/airborne/modules/ins/ins_xsens700.c
+++ b/sw/airborne/modules/ins/ins_xsens700.c
@@ -26,7 +26,6 @@
  */
 
 #include "ins_xsens700.h"
-#include "xsens_common.h"
 #include "subsystems/ins.h"
 
 #include "generated/airframe.h"
@@ -80,11 +79,11 @@ void ins_xsens700_init(void)
 
 void ins_xsens700_event(void)
 {
-  xsens_event();
-  if (xsens700.msg_received) {
+  xsens_parser_event(&(xsens700.parser));
+  if (xsens700.parser.msg_received) {
     parse_xsens700_msg();
     handle_ins_msg();
-    xsens700.msg_received = false;
+    xsens700.parser.msg_received = false;
   }
 }
 
diff --git a/sw/airborne/modules/ins/xsens.c b/sw/airborne/modules/ins/xsens.c
index 09a2b65ed9..7e144245c4 100644
--- a/sw/airborne/modules/ins/xsens.c
+++ b/sw/airborne/modules/ins/xsens.c
@@ -24,7 +24,6 @@
  */
 
 #include "xsens.h"
-#include "xsens_common.h"
 
 #include "generated/airframe.h"
 #include "led.h"
@@ -113,7 +112,7 @@ void parse_xsens_buffer(uint8_t c);
 
 void xsens_init(void)
 {
-  xsens_status = UNINIT;
+  xsens.parser.status = UNINIT;
   xsens_configured = 20;
 
   xsens_msg_status = 0;
@@ -185,76 +184,76 @@ void xsens_periodic(void)
 void parse_xsens_msg(void)
 {
   uint8_t offset = 0;
-  if (xsens_id == XSENS_ReqOutputModeAck_ID) {
-    xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens_msg_buf);
-  } else if (xsens_id == XSENS_ReqOutputSettings_ID) {
-    xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf);
-  } else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) {
-    xsens_declination = DegOfRad(XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf));
+  if (xsens.parser.id == XSENS_ReqOutputModeAck_ID) {
+    xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens.parser.msg_buf);
+  } else if (xsens.parser.id == XSENS_ReqOutputSettings_ID) {
+    xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens.parser.msg_buf);
+  } else if (xsens.parser.id == XSENS_ReqMagneticDeclinationAck_ID) {
+    xsens_declination = DegOfRad(XSENS_ReqMagneticDeclinationAck_declination(xsens.parser.msg_buf));
 
     DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice, &xsens_declination, &xsens_declination, &xsens_gps_arm_x,
                                    &xsens_gps_arm_y, &xsens_gps_arm_z);
-  } else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
-    xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
-    xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
-    xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);
+  } else if (xsens.parser.id == XSENS_ReqLeverArmGpsAck_ID) {
+    xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens.parser.msg_buf);
+    xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens.parser.msg_buf);
+    xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens.parser.msg_buf);
 
     DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice, &xsens_declination, &xsens_declination, &xsens_gps_arm_x,
                                    &xsens_gps_arm_y, &xsens_gps_arm_z);
-  } else if (xsens_id == XSENS_Error_ID) {
-    xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
+  } else if (xsens.parser.id == XSENS_Error_ID) {
+    xsens_errorcode = XSENS_Error_errorcode(xsens.parser.msg_buf);
   }
 #if USE_GPS_XSENS
-  else if (xsens_id == XSENS_GPSStatus_ID) {
-    xsens.gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
+  else if (xsens.parser.id == XSENS_GPSStatus_ID) {
+    xsens.gps.nb_channels = XSENS_GPSStatus_nch(xsens.parser.msg_buf);
     xsens.gps.num_sv = 0;
 
     uint8_t i;
     // Do not write outside buffer
     for (i = 0; i < Min(xsens.gps.nb_channels, GPS_NB_CHANNELS); i++) {
-      uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf, i);
+      uint8_t ch = XSENS_GPSStatus_chn(xsens.parser.msg_buf, i);
       if (ch > xsens.gps.nb_channels) { continue; }
-      xsens.gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
-      xsens.gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
-      xsens.gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
-      xsens.gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
+      xsens.gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens.parser.msg_buf, i);
+      xsens.gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens.parser.msg_buf, i);
+      xsens.gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens.parser.msg_buf, i);
+      xsens.gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens.parser.msg_buf, i);
       if (xsens.gps.svinfos[ch].flags > 0) {
         xsens.gps.num_sv++;
       }
     }
   }
 #endif
-  else if (xsens_id == XSENS_MTData_ID) {
+  else if (xsens.parser.id == XSENS_MTData_ID) {
     /* test RAW modes else calibrated modes */
     //if ((XSENS_MASK_RAWInertial(xsens_output_mode)) || (XSENS_MASK_RAWGPS(xsens_output_mode))) {
     if (XSENS_MASK_RAWInertial(xsens_output_mode)) {
       /* should we write raw data to separate struct? */
-      xsens.gyro.p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf, offset);
-      xsens.gyro.q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf, offset);
-      xsens.gyro.r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf, offset);
+      xsens.gyro.p = XSENS_DATA_RAWInertial_gyrX(xsens.parser.msg_buf, offset);
+      xsens.gyro.q = XSENS_DATA_RAWInertial_gyrY(xsens.parser.msg_buf, offset);
+      xsens.gyro.r = XSENS_DATA_RAWInertial_gyrZ(xsens.parser.msg_buf, offset);
       xsens.gyro_available = TRUE;
       offset += XSENS_DATA_RAWInertial_LENGTH;
     }
     if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
 #if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS
       xsens.gps.week = 0; // FIXME
-      xsens.gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf, offset) * 10;
-      xsens.gps.lla_pos.lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf, offset);
-      xsens.gps.lla_pos.lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf, offset);
-      xsens.gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset);
+      xsens.gps.tow = XSENS_DATA_RAWGPS_itow(xsens.parser.msg_buf, offset) * 10;
+      xsens.gps.lla_pos.lat = XSENS_DATA_RAWGPS_lat(xsens.parser.msg_buf, offset);
+      xsens.gps.lla_pos.lon = XSENS_DATA_RAWGPS_lon(xsens.parser.msg_buf, offset);
+      xsens.gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens.parser.msg_buf, offset);
       SetBit(xsens.gps.valid_fields, GPS_VALID_POS_LLA_BIT);
 
       // Compute geoid (MSL) height
       uint32_t hmsl = wgs84_ellipsoid_to_geoid_i(xsens.gps.lla_pos.lat, xsens.gps.lla_pos.lon);
-      xsens.gps.hmsl =  XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset) - hmsl;
+      xsens.gps.hmsl =  XSENS_DATA_RAWGPS_alt(xsens.parser.msg_buf, offset) - hmsl;
       SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
 
-      xsens.gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf, offset);
-      xsens.gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf, offset);
-      xsens.gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf, offset);
+      xsens.gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens.parser.msg_buf, offset);
+      xsens.gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens.parser.msg_buf, offset);
+      xsens.gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens.parser.msg_buf, offset);
       SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
-      xsens.gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf, offset) / 100;
-      xsens.gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf, offset) / 100;
+      xsens.gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens.parser.msg_buf, offset) / 100;
+      xsens.gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens.parser.msg_buf, offset) / 100;
       xsens.gps.pdop = 5;  // FIXME Not output by XSens
 
       xsens.gps_available = TRUE;
@@ -268,23 +267,23 @@ void parse_xsens_msg(void)
     if (XSENS_MASK_Calibrated(xsens_output_mode)) {
       uint8_t l = 0;
       if (!XSENS_MASK_AccOut(xsens_output_settings)) {
-        xsens.accel.x = XSENS_DATA_Calibrated_accX(xsens_msg_buf, offset);
-        xsens.accel.y = XSENS_DATA_Calibrated_accY(xsens_msg_buf, offset);
-        xsens.accel.z = XSENS_DATA_Calibrated_accZ(xsens_msg_buf, offset);
+        xsens.accel.x = XSENS_DATA_Calibrated_accX(xsens.parser.msg_buf, offset);
+        xsens.accel.y = XSENS_DATA_Calibrated_accY(xsens.parser.msg_buf, offset);
+        xsens.accel.z = XSENS_DATA_Calibrated_accZ(xsens.parser.msg_buf, offset);
         xsens.accel_available = TRUE;
         l++;
       }
       if (!XSENS_MASK_GyrOut(xsens_output_settings)) {
-        xsens.gyro.p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf, offset);
-        xsens.gyro.q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf, offset);
-        xsens.gyro.r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf, offset);
+        xsens.gyro.p = XSENS_DATA_Calibrated_gyrX(xsens.parser.msg_buf, offset);
+        xsens.gyro.q = XSENS_DATA_Calibrated_gyrY(xsens.parser.msg_buf, offset);
+        xsens.gyro.r = XSENS_DATA_Calibrated_gyrZ(xsens.parser.msg_buf, offset);
         xsens.gyro_available = TRUE;
         l++;
       }
       if (!XSENS_MASK_MagOut(xsens_output_settings)) {
-        xsens.mag.x = XSENS_DATA_Calibrated_magX(xsens_msg_buf, offset);
-        xsens.mag.y = XSENS_DATA_Calibrated_magY(xsens_msg_buf, offset);
-        xsens.mag.z = XSENS_DATA_Calibrated_magZ(xsens_msg_buf, offset);
+        xsens.mag.x = XSENS_DATA_Calibrated_magX(xsens.parser.msg_buf, offset);
+        xsens.mag.y = XSENS_DATA_Calibrated_magY(xsens.parser.msg_buf, offset);
+        xsens.mag.z = XSENS_DATA_Calibrated_magZ(xsens.parser.msg_buf, offset);
         xsens.mag_available = TRUE;
         l++;
       }
@@ -292,17 +291,17 @@ void parse_xsens_msg(void)
     }
     if (XSENS_MASK_Orientation(xsens_output_mode)) {
       if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) {
-        xsens.quat.qi = XSENS_DATA_Quaternion_q0(xsens_msg_buf, offset);
-        xsens.quat.qx = XSENS_DATA_Quaternion_q1(xsens_msg_buf, offset);
-        xsens.quat.qy = XSENS_DATA_Quaternion_q2(xsens_msg_buf, offset);
-        xsens.quat.qz = XSENS_DATA_Quaternion_q3(xsens_msg_buf, offset);
+        xsens.quat.qi = XSENS_DATA_Quaternion_q0(xsens.parser.msg_buf, offset);
+        xsens.quat.qx = XSENS_DATA_Quaternion_q1(xsens.parser.msg_buf, offset);
+        xsens.quat.qy = XSENS_DATA_Quaternion_q2(xsens.parser.msg_buf, offset);
+        xsens.quat.qz = XSENS_DATA_Quaternion_q3(xsens.parser.msg_buf, offset);
         //float_eulers_of_quat(&xsens.euler, &xsens.quat);
         offset += XSENS_DATA_Quaternion_LENGTH;
       }
       if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) {
-        xsens.euler.phi   = XSENS_DATA_Euler_roll(xsens_msg_buf, offset) * M_PI / 180;
-        xsens.euler.theta = XSENS_DATA_Euler_pitch(xsens_msg_buf, offset) * M_PI / 180;
-        xsens.euler.psi   = XSENS_DATA_Euler_yaw(xsens_msg_buf, offset) * M_PI / 180;
+        xsens.euler.phi   = XSENS_DATA_Euler_roll(xsens.parser.msg_buf, offset) * M_PI / 180;
+        xsens.euler.theta = XSENS_DATA_Euler_pitch(xsens.parser.msg_buf, offset) * M_PI / 180;
+        xsens.euler.psi   = XSENS_DATA_Euler_yaw(xsens.parser.msg_buf, offset) * M_PI / 180;
         offset += XSENS_DATA_Euler_LENGTH;
       }
       if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
@@ -321,9 +320,9 @@ void parse_xsens_msg(void)
       offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
     }
     if (XSENS_MASK_Position(xsens_output_mode)) {
-      xsens.lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf, offset));
-      xsens.lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf, offset));
-      xsens.lla_f.alt = XSENS_DATA_Position_alt(xsens_msg_buf, offset);
+      xsens.lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens.parser.msg_buf, offset));
+      xsens.lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens.parser.msg_buf, offset));
+      xsens.lla_f.alt = XSENS_DATA_Position_alt(xsens.parser.msg_buf, offset);
       offset += XSENS_DATA_Position_LENGTH;
 
 #if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
@@ -333,13 +332,13 @@ void parse_xsens_msg(void)
 #endif
     }
     if (XSENS_MASK_Velocity(xsens_output_mode)) {
-      xsens.vel.x = XSENS_DATA_Velocity_vx(xsens_msg_buf, offset);
-      xsens.vel.y = XSENS_DATA_Velocity_vy(xsens_msg_buf, offset);
-      xsens.vel.z = XSENS_DATA_Velocity_vz(xsens_msg_buf, offset);
+      xsens.vel.x = XSENS_DATA_Velocity_vx(xsens.parser.msg_buf, offset);
+      xsens.vel.y = XSENS_DATA_Velocity_vy(xsens.parser.msg_buf, offset);
+      xsens.vel.z = XSENS_DATA_Velocity_vz(xsens.parser.msg_buf, offset);
       offset += XSENS_DATA_Velocity_LENGTH;
     }
     if (XSENS_MASK_Status(xsens_output_mode)) {
-      xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf, offset);
+      xsens_msg_status = XSENS_DATA_Status_status(xsens.parser.msg_buf, offset);
 #if USE_GPS_XSENS
       if (bit_is_set(xsens_msg_status, 2)) { xsens.gps.fix = GPS_FIX_3D; } // gps fix
       else if (bit_is_set(xsens_msg_status, 1)) { xsens.gps.fix = 0x01; } // efk valid
@@ -355,17 +354,17 @@ void parse_xsens_msg(void)
       offset += XSENS_DATA_Status_LENGTH;
     }
     if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
-      xsens.time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf, offset);
+      xsens.time_stamp = XSENS_DATA_TimeStamp_ts(xsens.parser.msg_buf, offset);
       offset += XSENS_DATA_TimeStamp_LENGTH;
     }
     if (XSENS_MASK_UTC(xsens_output_settings)) {
-      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);
+      xsens.time.hour = XSENS_DATA_UTC_hour(xsens.parser.msg_buf, offset);
+      xsens.time.min = XSENS_DATA_UTC_min(xsens.parser.msg_buf, offset);
+      xsens.time.sec = XSENS_DATA_UTC_sec(xsens.parser.msg_buf, offset);
+      xsens.time.nanosec = XSENS_DATA_UTC_nanosec(xsens.parser.msg_buf, offset);
+      xsens.time.year = XSENS_DATA_UTC_year(xsens.parser.msg_buf, offset);
+      xsens.time.month = XSENS_DATA_UTC_month(xsens.parser.msg_buf, offset);
+      xsens.time.day = XSENS_DATA_UTC_day(xsens.parser.msg_buf, offset);
 
       offset += XSENS_DATA_UTC_LENGTH;
     }
diff --git a/sw/airborne/modules/ins/xsens.h b/sw/airborne/modules/ins/xsens.h
index 9e7fd48f79..795748a626 100644
--- a/sw/airborne/modules/ins/xsens.h
+++ b/sw/airborne/modules/ins/xsens.h
@@ -32,6 +32,9 @@
 #include "math/pprz_geodetic_float.h"
 #include "math/pprz_geodetic_int.h"
 
+#include "xsens_parser.h"
+
+
 #if USE_GPS_XSENS
 #include "subsystems/gps.h"
 #endif
@@ -60,7 +63,7 @@ struct Xsens {
   struct FloatQuat quat;
   struct FloatEulers euler;
 
-  volatile bool msg_received;
+  struct XsensParser parser;
   volatile bool new_attitude;
 
   bool gyro_available;
diff --git a/sw/airborne/modules/ins/xsens700.c b/sw/airborne/modules/ins/xsens700.c
index 6e39a752c9..6114cba73b 100644
--- a/sw/airborne/modules/ins/xsens700.c
+++ b/sw/airborne/modules/ins/xsens700.c
@@ -26,20 +26,13 @@
  */
 
 #include "xsens700.h"
-#include "xsens_common.h"
 
 #include "generated/airframe.h"
-#include "led.h"
 
 #if USE_GPS_XSENS
 #include "math/pprz_geodetic_wgs84.h"
 #endif
 
-// FIXME Debugging Only
-#include "mcu_periph/uart.h"
-#include "pprzlink/messages.h"
-#include "subsystems/datalink/downlink.h"
-
 
 uint8_t xsens_errorcode;
 uint32_t xsens_msg_statusword;
@@ -57,7 +50,7 @@ volatile int xsens_configured = 0;
 
 void xsens700_init(void)
 {
-  xsens_status = UNINIT;
+  xsens.parser.status = UNINIT;
   xsens_configured = 30;
 
   xsens_msg_statusword = 0;
@@ -146,17 +139,17 @@ void xsens700_periodic(void)
 void parse_xsens700_msg(void)
 {
   uint8_t offset = 0;
-  if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
-    xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
-    xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
-    xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);
-  } else if (xsens_id == XSENS_Error_ID) {
-    xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
-  } else if (xsens_id == XSENS_MTData2_ID) {
-    for (offset = 0; offset < xsens_len;) {
-      uint8_t code1 = xsens_msg_buf[offset];
-      uint8_t code2 = xsens_msg_buf[offset + 1];
-      int subpacklen = (int)xsens_msg_buf[offset + 2];
+  if (xsens.parser.id == XSENS_ReqLeverArmGpsAck_ID) {
+    xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens.parser.msg_buf);
+    xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens.parser.msg_buf);
+    xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens.parser.msg_buf);
+  } else if (xsens.parser.id == XSENS_Error_ID) {
+    xsens_errorcode = XSENS_Error_errorcode(xsens.parser.msg_buf);
+  } else if (xsens.parser.id == XSENS_MTData2_ID) {
+    for (offset = 0; offset < xsens.parser.len;) {
+      uint8_t code1 = xsens.parser.msg_buf[offset];
+      uint8_t code2 = xsens.parser.msg_buf[offset + 1];
+      int subpacklen = (int)xsens.parser.msg_buf[offset + 2];
       offset += 3;
 
       if (code1 == 0x10) {
@@ -171,25 +164,25 @@ void parse_xsens700_msg(void)
       } else if (code1 == 0x20) {
         if (code2 == 0x30) {
           // Attitude Euler
-          xsens700.euler.phi   = XSENS_DATA_Euler_roll(xsens_msg_buf, offset) * M_PI / 180;
-          xsens700.euler.theta = XSENS_DATA_Euler_pitch(xsens_msg_buf, offset) * M_PI / 180;
-          xsens700.euler.psi   = XSENS_DATA_Euler_yaw(xsens_msg_buf, offset) * M_PI / 180;
+          xsens700.euler.phi   = XSENS_DATA_Euler_roll(xsens.parser.msg_buf, offset) * M_PI / 180;
+          xsens700.euler.theta = XSENS_DATA_Euler_pitch(xsens.parser.msg_buf, offset) * M_PI / 180;
+          xsens700.euler.psi   = XSENS_DATA_Euler_yaw(xsens.parser.msg_buf, offset) * M_PI / 180;
 
           xsens700.new_attitude = TRUE;
         }
       } else if (code1 == 0x40) {
         if (code2 == 0x10) {
           // Delta-V
-          xsens700.accel.x = XSENS_DATA_Acceleration_x(xsens_msg_buf, offset) * 100.0f;
-          xsens700.accel.y = XSENS_DATA_Acceleration_y(xsens_msg_buf, offset) * 100.0f;
-          xsens700.accel.z = XSENS_DATA_Acceleration_z(xsens_msg_buf, offset) * 100.0f;
+          xsens700.accel.x = XSENS_DATA_Acceleration_x(xsens.parser.msg_buf, offset) * 100.0f;
+          xsens700.accel.y = XSENS_DATA_Acceleration_y(xsens.parser.msg_buf, offset) * 100.0f;
+          xsens700.accel.z = XSENS_DATA_Acceleration_z(xsens.parser.msg_buf, offset) * 100.0f;
         }
       } else if (code1 == 0x80) {
         if (code2 == 0x20) {
           // Rate Of Turn
-          xsens700.gyro.p = XSENS_DATA_RateOfTurn_x(xsens_msg_buf, offset) * M_PI / 180;
-          xsens700.gyro.q = XSENS_DATA_RateOfTurn_y(xsens_msg_buf, offset) * M_PI / 180;
-          xsens700.gyro.r = XSENS_DATA_RateOfTurn_z(xsens_msg_buf, offset) * M_PI / 180;
+          xsens700.gyro.p = XSENS_DATA_RateOfTurn_x(xsens.parser.msg_buf, offset) * M_PI / 180;
+          xsens700.gyro.q = XSENS_DATA_RateOfTurn_y(xsens.parser.msg_buf, offset) * M_PI / 180;
+          xsens700.gyro.r = XSENS_DATA_RateOfTurn_z(xsens.parser.msg_buf, offset) * M_PI / 180;
         }
       } else if (code1 == 0x30) {
         if (code2 == 0x10) {
@@ -198,7 +191,7 @@ void parse_xsens700_msg(void)
       } else if (code1 == 0xE0) {
         if (code2 == 0x20) {
           // Status Word
-          xsens_msg_statusword = XSENS_DATA_StatusWord_status(xsens_msg_buf, offset);
+          xsens_msg_statusword = XSENS_DATA_StatusWord_status(xsens.parser.msg_buf, offset);
           //xsens700.gps.tow = xsens_msg_statusword;
 #if USE_GPS_XSENS
           if (bit_is_set(xsens_msg_statusword, 2) && bit_is_set(xsens_msg_statusword, 1)) {
@@ -212,17 +205,17 @@ void parse_xsens700_msg(void)
         }
       } else if (code1 == 0x88) {
         if (code2 == 0x40) {
-          xsens700.gps.week = XSENS_DATA_GpsSol_week(xsens_msg_buf, offset);
-          xsens700.gps.num_sv = XSENS_DATA_GpsSol_numSv(xsens_msg_buf, offset);
-          xsens700.gps.pacc = XSENS_DATA_GpsSol_pAcc(xsens_msg_buf, offset);
-          xsens700.gps.sacc = XSENS_DATA_GpsSol_sAcc(xsens_msg_buf, offset);
-          xsens700.gps.pdop = XSENS_DATA_GpsSol_pDop(xsens_msg_buf, offset);
+          xsens700.gps.week = XSENS_DATA_GpsSol_week(xsens.parser.msg_buf, offset);
+          xsens700.gps.num_sv = XSENS_DATA_GpsSol_numSv(xsens.parser.msg_buf, offset);
+          xsens700.gps.pacc = XSENS_DATA_GpsSol_pAcc(xsens.parser.msg_buf, offset);
+          xsens700.gps.sacc = XSENS_DATA_GpsSol_sAcc(xsens.parser.msg_buf, offset);
+          xsens700.gps.pdop = XSENS_DATA_GpsSol_pDop(xsens.parser.msg_buf, offset);
         } else if (code2 == 0xA0) {
           // SVINFO
-          xsens700.gps.tow = XSENS_XDI_GpsSvInfo_iTOW(xsens_msg_buf + offset);
+          xsens700.gps.tow = XSENS_XDI_GpsSvInfo_iTOW(xsens.parser.msg_buf + offset);
 
 #if USE_GPS_XSENS
-          xsens700.gps.nb_channels = XSENS_XDI_GpsSvInfo_nch(xsens_msg_buf + offset);
+          xsens700.gps.nb_channels = XSENS_XDI_GpsSvInfo_nch(xsens.parser.msg_buf + offset);
 
           xsens700.gps.last_3dfix_ticks = sys_time.nb_sec_rem;
           xsens700.gps.last_3dfix_time = sys_time.nb_sec;
@@ -230,21 +223,21 @@ void parse_xsens700_msg(void)
           uint8_t i;
           // Do not write outside buffer
           for (i = 0; i < Min(xsens700.gps.nb_channels, GPS_NB_CHANNELS); i++) {
-            uint8_t ch = XSENS_XDI_GpsSvInfo_chn(xsens_msg_buf + offset, i);
+            uint8_t ch = XSENS_XDI_GpsSvInfo_chn(xsens.parser.msg_buf + offset, i);
             if (ch > xsens700.gps.nb_channels) { continue; }
-            xsens700.gps.svinfos[ch].svid = XSENS_XDI_GpsSvInfo_svid(xsens_msg_buf + offset, i);
-            xsens700.gps.svinfos[ch].flags = XSENS_XDI_GpsSvInfo_bitmask(xsens_msg_buf + offset, i);
-            xsens700.gps.svinfos[ch].qi = XSENS_XDI_GpsSvInfo_qi(xsens_msg_buf + offset, i);
-            xsens700.gps.svinfos[ch].cno = XSENS_XDI_GpsSvInfo_cnr(xsens_msg_buf + offset, i);
+            xsens700.gps.svinfos[ch].svid = XSENS_XDI_GpsSvInfo_svid(xsens.parser.msg_buf + offset, i);
+            xsens700.gps.svinfos[ch].flags = XSENS_XDI_GpsSvInfo_bitmask(xsens.parser.msg_buf + offset, i);
+            xsens700.gps.svinfos[ch].qi = XSENS_XDI_GpsSvInfo_qi(xsens.parser.msg_buf + offset, i);
+            xsens700.gps.svinfos[ch].cno = XSENS_XDI_GpsSvInfo_cnr(xsens.parser.msg_buf + offset, i);
           }
 #endif
         }
       } else if (code1 == 0x50) {
         if (code2 == 0x10) {
-          //xsens700.gps.hmsl = XSENS_DATA_Altitude_h(xsens_msg_buf,offset)* 1000.0f;
+          //xsens700.gps.hmsl = XSENS_DATA_Altitude_h(xsens.parser.msg_buf,offset)* 1000.0f;
         } else if (code2 == 0x20) {
           // Altitude Elipsoid
-          xsens700.gps.lla_pos.alt = XSENS_DATA_Altitude_h(xsens_msg_buf, offset) * 1000.0f;
+          xsens700.gps.lla_pos.alt = XSENS_DATA_Altitude_h(xsens.parser.msg_buf, offset) * 1000.0f;
 
           // Compute geoid (MSL) height
           float geoid_h = wgs84_ellipsoid_to_geoid_f(xsens700.lla_f.lat, xsens700.lla_f.lon);
@@ -261,15 +254,15 @@ void parse_xsens700_msg(void)
           xsens700.gps.last_3dfix_time = sys_time.nb_sec;
           xsens700.gps.week = 0; // FIXME
 
-          xsens700.lla_f.lat = RadOfDeg(XSENS_DATA_LatLon_lat(xsens_msg_buf, offset));
-          xsens700.lla_f.lon = RadOfDeg(XSENS_DATA_LatLon_lon(xsens_msg_buf, offset));
+          xsens700.lla_f.lat = RadOfDeg(XSENS_DATA_LatLon_lat(xsens.parser.msg_buf, offset));
+          xsens700.lla_f.lon = RadOfDeg(XSENS_DATA_LatLon_lon(xsens.parser.msg_buf, offset));
         }
       } else if (code1 == 0xD0) {
         if (code2 == 0x10) {
           // Velocity
-          xsens700.vel.x = XSENS_DATA_VelocityXYZ_x(xsens_msg_buf, offset);
-          xsens700.vel.y = XSENS_DATA_VelocityXYZ_y(xsens_msg_buf, offset);
-          xsens700.vel.z = XSENS_DATA_VelocityXYZ_z(xsens_msg_buf, offset);
+          xsens700.vel.x = XSENS_DATA_VelocityXYZ_x(xsens.parser.msg_buf, offset);
+          xsens700.vel.y = XSENS_DATA_VelocityXYZ_y(xsens.parser.msg_buf, offset);
+          xsens700.vel.z = XSENS_DATA_VelocityXYZ_z(xsens.parser.msg_buf, offset);
           xsens700.gps.ned_vel.x = xsens700.vel.x;
           xsens700.gps.ned_vel.y = xsens700.vel.y;
           xsens700.gps.ned_vel.z = xsens700.vel.x;
diff --git a/sw/airborne/modules/ins/xsens700.h b/sw/airborne/modules/ins/xsens700.h
index 7d4d919f03..af3d50609a 100644
--- a/sw/airborne/modules/ins/xsens700.h
+++ b/sw/airborne/modules/ins/xsens700.h
@@ -32,6 +32,9 @@
 #include "math/pprz_geodetic_float.h"
 #include "math/pprz_geodetic_int.h"
 
+#include "xsens_parser.h"
+
+
 #if USE_GPS_XSENS
 #include "subsystems/gps.h"
 #endif
@@ -60,7 +63,7 @@ struct Xsens {
   struct FloatQuat quat;
   struct FloatEulers euler;
 
-  volatile bool msg_received;
+  struct XsensParser parser;
   volatile bool new_attitude;
 
   bool gyro_available;
diff --git a/sw/airborne/modules/ins/xsens_common.c b/sw/airborne/modules/ins/xsens_parser.c
similarity index 63%
rename from sw/airborne/modules/ins/xsens_common.c
rename to sw/airborne/modules/ins/xsens_parser.c
index 4c2c9ba95a..eb0f3a5117 100644
--- a/sw/airborne/modules/ins/xsens_common.c
+++ b/sw/airborne/modules/ins/xsens_parser.c
@@ -23,75 +23,67 @@
  * Parser for the Xsens protocol.
  */
 
-#include "xsens_common.h"
+#include "xsens_parser.h"
 
 #include "pprzlink/pprzlink_device.h"
 #include "mcu_periph/uart.h"
 
-volatile uint8_t xsens_msg_received;
-
-uint8_t xsens_id;
-uint8_t xsens_status;
-uint8_t xsens_len;
-uint8_t xsens_msg_idx;
-uint8_t ck;
 uint8_t send_ck;
 
-uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD];
+void xsens_parser_func(struct XsensParser *xsens, uint8_t c);
 
-void parse_xsens_buffer(uint8_t c);
-
-void xsens_event(void)
+void xsens_parser_event(struct XsensParser *xsensparser)
 {
   struct link_device *dev = &((XSENS_LINK).device);
   if (dev->char_available(dev->periph)) {
-    while (dev->char_available(dev->periph) && !xsens_msg_received) {
-      parse_xsens_buffer(dev->get_byte(dev->periph));
+    while (dev->char_available(dev->periph) && !xsensparser->msg_received) {
+      xsens_parser_func(xsensparser, dev->get_byte(dev->periph));
     }
   }
 }
 
-void parse_xsens_buffer(uint8_t c)
+void xsens_parser_func(struct XsensParser *xsens, uint8_t c)
 {
-  ck += c;
-  switch (xsens_status) {
+  xsens->ck += c;
+  switch (xsens->status) {
     case UNINIT:
       if (c != XSENS_START) {
         goto error;
       }
-      xsens_status++;
-      ck = 0;
+      xsens->status++;
+      xsens->ck = 0;
       break;
     case GOT_START:
       if (c != XSENS_BID) {
         goto error;
       }
-      xsens_status++;
+      xsens->status++;
       break;
     case GOT_BID:
-      xsens_id = c;
-      xsens_status++;
+      xsens->id = c;
+      xsens->status++;
       break;
     case GOT_MID:
-      xsens_len = c;
-      if (xsens_len > XSENS_MAX_PAYLOAD) {
+      xsens->len = c;
+      if (xsens->len > XSENS_MAX_PAYLOAD) {
         goto error;
       }
-      xsens_msg_idx = 0;
-      xsens_status++;
+      xsens->msg_idx = 0;
+      xsens->status++;
       break;
     case GOT_LEN:
-      xsens_msg_buf[xsens_msg_idx] = c;
-      xsens_msg_idx++;
-      if (xsens_msg_idx >= xsens_len) {
-        xsens_status++;
+      xsens->msg_buf[xsens->msg_idx] = c;
+      xsens->msg_idx++;
+      if (xsens->msg_idx >= xsens->len) {
+        xsens->status++;
       }
       break;
     case GOT_DATA:
-      if (ck != 0) {
+      if (xsens->ck != 0) {
         goto error;
       }
-      xsens_msg_received = TRUE;
+      xsens->msg_received = TRUE;
+
       goto restart;
       break;
     default:
@@ -100,6 +92,6 @@ void parse_xsens_buffer(uint8_t c)
   return;
 error:
 restart:
-  xsens_status = UNINIT;
+  xsens->status = UNINIT;
   return;
 }
diff --git a/sw/airborne/modules/ins/xsens_common.h b/sw/airborne/modules/ins/xsens_parser.h
similarity index 81%
rename from sw/airborne/modules/ins/xsens_common.h
rename to sw/airborne/modules/ins/xsens_parser.h
index c6f454ca6f..0e5f37c41f 100644
--- a/sw/airborne/modules/ins/xsens_common.h
+++ b/sw/airborne/modules/ins/xsens_parser.h
@@ -19,23 +19,42 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/** @file modules/ins/xsens_common.h
- * Parser for the Xsens protocol.
+/** @file modules/ins/xsens_parser.h
+ * Parser for the XSens protocol.
+ * Transmit XSens protocol.
  */
 
-#ifndef XSENS_COMMON_H
-#define XSENS_COMMON_H
+#ifndef XSENS_PARSER_H
+#define XSENS_PARSER_H
 
 #include "std.h"
 
 /** Includes macros generated from xsens_MTi-G.xml */
 #include "xsens_protocol.h"
 
-extern uint8_t xsens_id;
-extern uint8_t xsens_status;
-extern uint8_t xsens_len;
-extern uint8_t xsens_msg_idx;
-extern uint8_t ck;
+#define XSENS_MAX_PAYLOAD 254
+
+// Status
+#define UNINIT        0
+#define GOT_START     1
+#define GOT_BID       2
+#define GOT_MID       3
+#define GOT_LEN       4
+#define GOT_DATA      5
+#define GOT_CHECKSUM  6
+
+struct XsensParser {
+  volatile uint8_t msg_received;
+  uint8_t id;
+  uint8_t status;
+  uint8_t len;
+  uint8_t msg_idx;
+  uint8_t ck;
+  uint8_t msg_buf[XSENS_MAX_PAYLOAD];
+};
+
+
+// Only 1 send allowed at the same time:
 extern uint8_t send_ck;
 
 #define XsensLinkDevice (&((XSENS_LINK).device))
@@ -59,17 +78,6 @@ extern uint8_t send_ck;
 #define XsensTrailer() { uint8_t i8=0x100-send_ck; XsensUartSend1(i8); }
 
 
-#define XSENS_MAX_PAYLOAD 254
-extern uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD];
+extern void xsens_parser_event(struct XsensParser *xsensparser);
 
-#define UNINIT        0
-#define GOT_START     1
-#define GOT_BID       2
-#define GOT_MID       3
-#define GOT_LEN       4
-#define GOT_DATA      5
-#define GOT_CHECKSUM  6
-
-extern void xsens_event(void);
-
-#endif /* XSENS_COMMON_H */
+#endif /* XSENS_PARSER_H */