Add csc_xsens.c to parse xsens messages to csc_ap

conf/autopilot/csc_ap.makefile

@@ -60,6 +60,7 @@ ap.srcs += $(SRC_ARCH)/uart_hw.c
 ap.srcs += $(SRC_ARCH)/adc_hw.c
 ap.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_0 -DUSE_AD0_1
 
+
 ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B230400 -DUART0_VIC_SLOT=5
 ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUART1_VIC_SLOT=6
 ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport \
@@ -67,6 +68,8 @@ ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport \
 ap.srcs += downlink.c pprz_transport.c $(SRC_CSC)/csc_telemetry.c
 ap.srcs += $(SRC_CSC)/csc_datalink.c
 
+ap.srcs += $(SRC_CSC)/csc_xsens.c
+ap.CFLAGS += -DXSENS1_LINK=Uart0
 
 #ap.CFLAGS += -DAP_LINK_CAN -DCAN_LED=2
 #ap.CFLAGS += -DUSE_CAN1 -DCAN1_BTR=CANBitrate125k_2MHz

conf/telemetry/csc_ap.xml

@@ -4,15 +4,18 @@
   <process name="Ap">
     <mode name="default">
       <message name="ALIVE"          period="5"/>
-      <message name="ACTUATORS"      period="0.5"/>
       <message name="COMMANDS"       period="0.5"/>
-      <message name="RC"	    period="0.5"/>
-      <message name="PPM"       period="0.5"/>
       <message name="QUAD_STATUS"    period="1.1"/>
       <message name="DL_VALUE"       period="1.5"/>
+      <message name="IMU_GYRO"       period="1.2"/>
+      <message name="IMU_ACCEL"       period="1.2"/>
+      <message name="IMU_MAG"       period="1.2"/>
     </mode>
     <mode name="debug">
+      <message name="ACTUATORS"      period="0.5"/>
       <message name="QUAD_STATUS"    period="1.1"/>
+      <message name="RC"	     period="0.5"/>
+      <message name="PPM"	     period="0.5"/>
     </mode>
   </process>
 </telemetry>

sw/airborne/csc/csc_ap_main.c

@@ -41,6 +41,7 @@
 #include "csc_servos.h"
 #include "csc_telemetry.h"
 #include "csc_adc.h"
+#include "csc_xsens.h"
 
 #define CSC_STATUS_TIMEOUT (SYS_TICS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
 
@@ -75,6 +76,10 @@ STATIC_INLINE void csc_main_init( void ) {
   Uart0Init();
   Uart1Init();
 
+  xsens_init();
+
+
+
   csc_adc_init();
   ppm_init();
 
@@ -99,6 +104,7 @@ STATIC_INLINE void csc_main_periodic( void )
   if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) {
     csc_adc_periodic();
   }
+  xsens_periodic_task();
 
 #ifdef ACTUATORS
   SetActuatorsFromCommands(commands);
@@ -108,6 +114,7 @@ STATIC_INLINE void csc_main_periodic( void )
 
 STATIC_INLINE void csc_main_event( void )
 {
+  xsens_event_task();
   DatalinkEvent();
 #ifdef RADIO_CONTROL
   if (ppm_valid) {

sw/airborne/csc/csc_xsens.c

@@ -0,0 +1,393 @@
+/*
+ * Paparazzi mcu0 $Id: quad_xsens.c,v 1.2 2008/05/07 12:54:23 gautier Exp $
+ *  
+ * Copyright (C) 2003  Pascal Brisset, Antoine Drouin
+ *
+ * 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 csc_xsens.c
+ * \brief Parser for the Xsens protocol
+ */
+
+#include "csc_xsens.h"
+
+#include <inttypes.h>
+
+#include "led.h"
+
+#include "downlink.h"
+#include "messages.h"
+#include "uart.h"
+//#include "com_stats.h"
+
+void parse_xsens_msg(uint8_t xsens_id, uint8_t c );
+
+#define __Xsens2Link(dev, _x) dev##_x
+#define _Xsens2Link(dev, _x)  __Xsens2Link(dev, _x)
+#define Xsens2Link(_x) _Xsens2Link(XSENS2_LINK, _x)
+
+#define Xsens2Buffer() Xsens2Link(ChAvailable())
+#define ReadXsens2Buffer() { while (Xsens2Link(ChAvailable())&&!xsens_msg_received[1]) parse_xsens_msg(1, Xsens2Link(Getch())); }
+
+#define Xsens2UartSend1(c) Xsens2Link(Transmit(c))
+#define Xsens2UartInitParam(_a,_b,_c) Xsens2Link(InitParam(_a,_b,_c))
+#define Xsens2UartRunning Xsens2Link(TxRunning)
+
+#define Xsens2InitCheksum() { send_ck[1] = 0; }
+#define Xsens2UpdateChecksum(c) { send_ck[1] += c; }
+
+#define Xsens2Send1(c) { uint8_t i8=c; XSens2UartSend1(i8); Xsens2UpdateChecksum(i8); }
+#define Xsens2Send1ByAddr(x) { Xsens2Send1(*x); }
+#define Xsens2Send2ByAddr(x) { Xsens2Send1(*(x+1)); Xsens2Send1(*x); }
+#define Xsens2Send4ByAddr(x) { Xsens2Send1(*(x+3)); Xsens2Send1(*(x+2)); Xsens2Send1(*(x+1)); Xsens2Send1(*x); }
+
+#define Xsens2Header(msg_id, len) { \
+  Xsens2UartSend1(XSENS_START); \
+  Xsens2InitCheksum(); \
+  Xsens2Send1(XSENS_BID); \
+  Xsens2Send1(msg_id); \
+  Xsens2Send1(len); \
+}
+#define Xsens2Trailer() { uint8_t i8=0x100-send_ck[1]; Xsens2UartSend1(i8); }
+
+#define __Xsens1Link(dev, _x) dev##_x
+#define _Xsens1Link(dev, _x)  __Xsens1Link(dev, _x)
+#define Xsens1Link(_x) _Xsens1Link(XSENS1_LINK, _x)
+
+#define Xsens1Buffer() Xsens1Link(ChAvailable())
+#define ReadXsens1Buffer() { while (Xsens1Link(ChAvailable())&&!xsens_msg_received[0]) parse_xsens_msg(0, Xsens1Link(Getch())); }
+
+#define Xsens1UartSend1(c) Xsens1Link(Transmit(c))
+#define Xsens1UartInitParam(_a,_b,_c) Xsens1Link(InitParam(_a,_b,_c))
+#define Xsens1UartRunning Xsens1Link(TxRunning)
+
+#define Xsens1InitCheksum() { send_ck[0] = 0; }
+#define Xsens1UpdateChecksum(c) { send_ck[0] += c; }
+
+#define Xsens1Send1(c) { uint8_t i8=c; XSens1UartSend1(i8); Xsens1UpdateChecksum(i8); }
+#define Xsens1Send1ByAddr(x) { Xsens1Send1(*x); }
+#define Xsens1Send2ByAddr(x) { Xsens1Send1(*(x+1)); Xsens1Send1(*x); }
+#define Xsens1Send4ByAddr(x) { Xsens1Send1(*(x+3)); Xsens1Send1(*(x+2)); Xsens1Send1(*(x+1)); Xsens1Send1(*x); }
+
+#define Xsens1Header(msg_id, len) { \
+  Xsens1UartSend1(XSENS_START); \
+  Xsens1InitCheksum(); \
+  Xsens1Send1(XSENS_BID); \
+  Xsens1Send1(msg_id); \
+  Xsens1Send1(len); \
+}
+#define Xsens1Trailer() { uint8_t i8=0x100-send_ck[0]; Xsens1UartSend1(i8); }
+
+
+/** Includes macros generated from xsens_MTi.xml */ 
+#include "xsens_protocol.h"
+
+uint8_t xsens_mode[XSENS_COUNT]; // Receiver status 
+volatile uint8_t xsens_msg_received[XSENS_COUNT];
+
+float xsens_pitch[XSENS_COUNT];
+float xsens_roll[XSENS_COUNT];
+float xsens_yaw[XSENS_COUNT];
+
+float xsens_r_a[XSENS_COUNT];
+float xsens_r_b[XSENS_COUNT];
+float xsens_r_c[XSENS_COUNT];
+float xsens_r_d[XSENS_COUNT];
+float xsens_r_e[XSENS_COUNT];
+float xsens_r_f[XSENS_COUNT];
+float xsens_r_g[XSENS_COUNT];
+float xsens_r_h[XSENS_COUNT];
+float xsens_r_i[XSENS_COUNT];
+
+float xsens_accel_x[XSENS_COUNT];
+float xsens_accel_y[XSENS_COUNT];
+float xsens_accel_z[XSENS_COUNT];
+
+float xsens_mag_x[XSENS_COUNT];
+float xsens_mag_y[XSENS_COUNT];
+float xsens_mag_z[XSENS_COUNT];
+
+float xsens_gyro_x[XSENS_COUNT];
+float xsens_gyro_y[XSENS_COUNT];
+float xsens_gyro_z[XSENS_COUNT];
+
+float xsens_mag_heading[XSENS_COUNT];
+
+#define XSENS_MSG_BUF 2
+#define XSENS_MAX_PAYLOAD 254
+uint8_t xsens_msg_buf[XSENS_COUNT][XSENS_MSG_BUF][XSENS_MAX_PAYLOAD];
+static volatile uint8_t xsens_msg_buf_count[XSENS_COUNT]; // buffer count
+static volatile uint8_t xsens_msg_buf_pi[XSENS_COUNT]; // produce index
+static volatile uint8_t xsens_msg_buf_ci[XSENS_COUNT]; // consume index
+
+// See Page 25 MT Low-Level Comm Doc
+#define XSENS_OUTPUT_CALIBRATED (1 << 1)
+#define XSENS_OUTPUT_ORIENTATION (1 << 2)
+#define XSENS_OUTPUT_AUXDATA (1 << 3)
+#define XSENS_OUTPUT_STATUS (1 << 11)
+#define XSENS_DEFAULT_OUTPUT_MODE (XSENS_OUTPUT_ORIENTATION | XSENS_OUTPUT_STATUS | XSENS_OUTPUT_CALIBRATED)
+
+// output settings : (Page 26 MT Low-Level Comm Doc)
+// Sample Counter, rotation matrix, floating point output
+#define XSENS_OUTPUT_AUX1 (1 << 10)
+#define XSENS_OUTPUT_AUX2 (1 << 11)
+#define XSENS_ORIENTATION (2 << 2)
+#define XSENS_ACCELS (1 << 4)
+#define XSENS_GYROS (1 << 5)
+#define XSENS_MAGS (1 << 6)
+#define XSENS_TIMESTAMP (1 << 0)
+#define XSENS_DEFAULT_OUTPUT_SETTINGS (XSENS_ORIENTATION | XSENS_ACCELS | XSENS_MAGS | XSENS_GYROS | XSENS_TIMESTAMP)
+
+#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
+
+uint8_t xsens_errorcode[XSENS_COUNT];
+uint8_t xsens_msg_status[XSENS_COUNT];
+uint16_t xsens_time_stamp[XSENS_COUNT];
+uint16_t xsens_output_mode[XSENS_COUNT];
+uint32_t xsens_output_settings[XSENS_COUNT];
+
+static uint8_t msg_id[XSENS_COUNT][XSENS_MSG_BUF];
+static uint8_t xsens_len[XSENS_COUNT][XSENS_MSG_BUF];
+static uint8_t xsens_msg_idx[XSENS_COUNT];
+static uint8_t ck[XSENS_COUNT];
+static uint8_t send_ck[XSENS_COUNT];
+
+void xsens_init( void )
+{
+  for (int i = 0; i < XSENS_COUNT; i++) {
+    send_ck[i] = 0;
+    xsens_msg_status[i] = 0;
+    xsens_time_stamp[i] = 0;
+    xsens_output_mode[i] = XSENS_DEFAULT_OUTPUT_MODE;
+    xsens_output_settings[i] = XSENS_DEFAULT_OUTPUT_SETTINGS;
+    xsens_msg_buf_count[i] = 0;
+    xsens_msg_buf_pi[i] = 0;
+    xsens_msg_buf_ci[i] = 0;
+  }
+  // Also TODO: set scenario to aerospace
+  // set magnetic declination angle
+  // Probably quicker to just set everything once via MT Manager software
+  // instead of setting via paparazzi (MT-G should store setting in flash)
+  //XSENS_GoToConfig();
+  //XSENS_SetOutputMode(xsens2_output_mode);
+  //XSENS_SetOutputSettings(xsens2_output_settings);
+  //XSENS_GoToMeasurment();
+}
+
+void xsens_periodic_task ( void )
+{
+  for (int i = 0; i < XSENS_COUNT; i++)
+  {
+    if (xsens_msg_buf_count[i] > 0) {
+      xsens_parse_msg(i);
+      xsens_msg_buf_count[i]--;
+      xsens_msg_buf_ci[i] = (xsens_msg_buf_ci[i] + 1) % XSENS_MSG_BUF;
+    }
+  }
+}
+
+void xsens_event_task( void ) 
+{
+  while (Xsens1Link(ChAvailable()) && !xsens_msg_received[0]) {
+    parse_xsens_msg(0, Xsens1Link(Getch()));
+  }
+
+/*
+  while (Xsens2Link(ChAvailable()) && !xsens_msg_received[1])
+    parse_xsens_msg(1, Xsens2Link(Getch()));
+*/
+
+  for (int i = 0; i < XSENS_COUNT; i++) {
+    if (xsens_msg_received[i]) {
+
+      // first make room
+      while (xsens_msg_buf_count[i] >= XSENS_MSG_BUF) {
+        // throwing away old stuff
+        xsens_msg_buf_ci[i] = (xsens_msg_buf_ci[i] + 1 ) % XSENS_MSG_BUF;
+        xsens_msg_buf_count[i]--;
+      }
+
+      xsens_msg_buf_pi[i] = (xsens_msg_buf_pi[i] + 1 ) % XSENS_MSG_BUF;
+      xsens_msg_buf_count[i]++;
+
+      //xsens_parse_msg(i);
+      xsens_msg_received[i] = FALSE;
+    }
+  }
+}
+
+// Called after receipt of valid message to 
+void xsens_parse_msg( uint8_t xsens_id ) {
+  uint8_t buf_slot = xsens_msg_buf_ci[xsens_id];
+
+  if (msg_id[xsens_id][buf_slot] == XSENS_ReqOutputModeAck_ID) {
+    xsens_output_mode[xsens_id] = XSENS_ReqOutputModeAck_mode(xsens_msg_buf[xsens_id]);
+  }
+  else if (msg_id[xsens_id][buf_slot] == XSENS_Error_ID) {
+    xsens_errorcode[xsens_id] = XSENS_Error_errorcode(xsens_msg_buf[xsens_id]);
+  }
+  else if (msg_id[xsens_id][buf_slot] == XSENS_MTData_ID) {
+    uint8_t offset = 0;
+    // test RAW modes else calibrated modes 
+    //if ((XSENS_MASK_RAWInertial(xsens2_output_mode)) || (XSENS_MASK_RAWGPS(xsens2_output_mode))) {
+      if (XSENS_MASK_Temp(xsens_output_mode[xsens_id])) {
+        offset += XSENS_DATA_Temp_LENGTH;
+      }
+      if (XSENS_MASK_Calibrated(xsens_output_mode[xsens_id])) {
+        if (XSENS_MASK_AccOut(xsens_output_settings[xsens_id])) {
+          xsens_accel_x[xsens_id] = XSENS_DATA_Calibrated_accX(xsens_msg_buf[xsens_id][buf_slot],offset);
+          xsens_accel_y[xsens_id] = XSENS_DATA_Calibrated_accY(xsens_msg_buf[xsens_id][buf_slot],offset);
+          xsens_accel_z[xsens_id] = XSENS_DATA_Calibrated_accZ(xsens_msg_buf[xsens_id][buf_slot],offset);
+        }
+        if (XSENS_MASK_GyrOut(xsens_output_settings[xsens_id])) {
+          xsens_gyro_x[xsens_id] = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf[xsens_id][buf_slot],offset);
+          xsens_gyro_y[xsens_id] = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf[xsens_id][buf_slot],offset);
+          xsens_gyro_z[xsens_id] = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf[xsens_id][buf_slot],offset);
+        }
+        if (XSENS_MASK_MagOut(xsens_output_settings[xsens_id])) {
+          xsens_mag_x[xsens_id] = XSENS_DATA_Calibrated_magX(xsens_msg_buf[xsens_id][buf_slot],offset);
+          xsens_mag_y[xsens_id] = XSENS_DATA_Calibrated_magY(xsens_msg_buf[xsens_id][buf_slot],offset);
+          xsens_mag_z[xsens_id] = XSENS_DATA_Calibrated_magZ(xsens_msg_buf[xsens_id][buf_slot],offset);
+          float pitch = xsens_roll[xsens_id];
+          float roll = -xsens_pitch[xsens_id];
+          float tilt_comp_x = xsens_mag_x[xsens_id] * cos(pitch)
+                            + xsens_mag_y[xsens_id] * sin(roll) * sin(pitch)
+                            - xsens_mag_z[xsens_id] * cos(roll) * sin(pitch);
+          float tilt_comp_y = xsens_mag_y[xsens_id] * cos(roll) 
+                            + xsens_mag_z[xsens_id] * sin(roll);
+          xsens_mag_heading[xsens_id] = -atan2( tilt_comp_y, tilt_comp_x);
+        }
+        offset += XSENS_DATA_Calibrated_LENGTH;
+      }
+      if (XSENS_MASK_Orientation(xsens_output_mode[xsens_id])) {
+        if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x0) {
+          offset += XSENS_DATA_Quaternion_LENGTH;
+        }
+        if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x1) {
+          xsens_roll[xsens_id] = XSENS_DATA_Euler_roll(xsens_msg_buf[xsens_id][buf_slot],offset)  * M_PI/180;
+          xsens_pitch[xsens_id] =XSENS_DATA_Euler_pitch(xsens_msg_buf[xsens_id][buf_slot],offset) * M_PI/180;
+          xsens_yaw[xsens_id] =  XSENS_DATA_Euler_yaw(xsens_msg_buf[xsens_id][buf_slot],offset)   * M_PI/180;
+          offset += XSENS_DATA_Euler_LENGTH;
+        }
+        if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x2) {
+          xsens_r_a[xsens_id] = XSENS_DATA_Matrix_a(xsens_msg_buf[xsens_id][buf_slot],offset);
+          xsens_r_b[xsens_id] = XSENS_DATA_Matrix_b(xsens_msg_buf[xsens_id][buf_slot],offset);
+          xsens_r_c[xsens_id] = XSENS_DATA_Matrix_c(xsens_msg_buf[xsens_id][buf_slot],offset);
+          xsens_r_d[xsens_id] = XSENS_DATA_Matrix_d(xsens_msg_buf[xsens_id][buf_slot],offset);
+          xsens_r_e[xsens_id] = XSENS_DATA_Matrix_e(xsens_msg_buf[xsens_id][buf_slot],offset);
+          xsens_r_f[xsens_id] = XSENS_DATA_Matrix_f(xsens_msg_buf[xsens_id][buf_slot],offset);
+          xsens_r_g[xsens_id] = XSENS_DATA_Matrix_g(xsens_msg_buf[xsens_id][buf_slot],offset);
+          xsens_r_h[xsens_id] = XSENS_DATA_Matrix_h(xsens_msg_buf[xsens_id][buf_slot],offset);
+          xsens_r_i[xsens_id] = XSENS_DATA_Matrix_i(xsens_msg_buf[xsens_id][buf_slot],offset);
+
+          // Calculate roll, pitch, yaw from rotation matrix ( p 31-33 MTi-G USer Man and Tech Doc)
+          xsens_pitch[xsens_id] = atan2 (xsens_r_f[xsens_id], xsens_r_i[xsens_id]);
+          xsens_roll[xsens_id] = asin (xsens_r_c[xsens_id]);
+          xsens_yaw[xsens_id] = atan2 (xsens_r_b[xsens_id], xsens_r_a[xsens_id]);
+
+          offset += XSENS_DATA_Matrix_LENGTH;
+        }
+      }
+      if (XSENS_MASK_Status(xsens_output_mode[xsens_id])) {
+        xsens_msg_status[xsens_id] = XSENS_DATA_Status_status(xsens_msg_buf[xsens_id][buf_slot],offset);
+        xsens_mode[xsens_id] = xsens_msg_status[xsens_id];
+        offset += XSENS_DATA_Status_LENGTH;
+      }
+      if (XSENS_MASK_TimeStamp(xsens_output_settings[xsens_id])) {
+        uint16_t ts = XSENS_DATA_TimeStamp_ts(xsens_msg_buf[xsens_id][buf_slot],offset);
+        if (xsens_time_stamp[xsens_id] + 1 != ts)
+          //xsens_err_count[xsens_id]++;
+        xsens_time_stamp[xsens_id] = ts;
+        offset += XSENS_DATA_TimeStamp_LENGTH;
+      }
+  }
+}
+
+
+// Simple state machine parser for XSENS messages (MT Low-Level Comm Doc)
+// Passed serial bytes one per call and parses stream into message id, data length, and data buffer
+// for use at higher level
+void parse_xsens_msg( uint8_t xsens_id, uint8_t c ) {
+  static uint8_t xsens_status[XSENS_COUNT];
+  uint8_t buf_slot = xsens_msg_buf_pi[xsens_id]; // produce index
+
+  // keep track of checksum byte
+  ck[xsens_id] += c;
+  switch (xsens_status[xsens_id]) {
+  case UNINIT:
+    // Look for xsens start byte 
+    if (c != XSENS_START)
+      goto error;
+    xsens_status[xsens_id]++; 
+    ck[xsens_id] = 0; // Reset checksum
+    break;
+  case GOT_START:
+    // Look for xsens Bus ID
+    if (c != XSENS_BID)
+      goto error;
+    xsens_status[xsens_id]++;
+    break;
+  case GOT_BID:
+    // Save message ID
+    msg_id[xsens_id][buf_slot] = c;
+    xsens_status[xsens_id]++;
+    break;
+  case GOT_MID:
+    // Save message length
+    xsens_len[xsens_id][buf_slot] = c;
+    // check for valid message length 
+    if (xsens_len[xsens_id][buf_slot] > XSENS_MAX_PAYLOAD)
+      goto error;
+    xsens_msg_idx[xsens_id] = 0; // Reset buffer index
+    xsens_status[xsens_id]++;
+    break;
+  case GOT_LEN:
+    // Read byte into data buffer
+    xsens_msg_buf[xsens_id][buf_slot][xsens_msg_idx[xsens_id]] = c;
+    xsens_msg_idx[xsens_id]++;
+    // Terminate reading at end of data field
+    if (xsens_msg_idx[xsens_id] >= xsens_len[xsens_id][buf_slot])
+      xsens_status[xsens_id]++;
+    break;
+  case GOT_DATA:
+    // Check for valid checksum
+    if (ck[xsens_id] != 0) {
+      //xsens_err_count[xsens_id]++;
+      goto error;
+    }
+    // Notification for valid message received
+    xsens_msg_received[xsens_id] = TRUE;
+    //xsens_recv_count[xsens_id]++;
+    goto restart;
+    break;
+  }
+  return;
+ error:  
+ restart:
+  // Start over (Reset parser state)
+  xsens_status[xsens_id] = UNINIT;
+  return;
+}

sw/airborne/csc/csc_xsens.h

@@ -0,0 +1,96 @@
+/*
+ * Copyright (C) 2008 Joby Energy
+ *  
+ *
+ * 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 xsens.h
+ */
+
+#ifndef __CSC_XSENS_H__
+#define __CSC_XSENS_H__
+
+#include "types.h"
+
+#define XSENS_COUNT 1
+
+extern uint8_t xsens_mode[XSENS_COUNT]; // Receiver status 
+extern volatile uint8_t xsens_msg_received[XSENS_COUNT];
+
+extern float xsens_pitch[XSENS_COUNT];
+extern float xsens_roll[XSENS_COUNT];
+extern float xsens_yaw[XSENS_COUNT];
+extern uint8_t xsens_msg_status[XSENS_COUNT];
+
+extern float xsens_r_a[XSENS_COUNT];
+extern float xsens_r_b[XSENS_COUNT];
+extern float xsens_r_c[XSENS_COUNT];
+extern float xsens_r_d[XSENS_COUNT];
+extern float xsens_r_e[XSENS_COUNT];
+extern float xsens_r_f[XSENS_COUNT];
+extern float xsens_r_g[XSENS_COUNT];
+extern float xsens_r_h[XSENS_COUNT];
+extern float xsens_r_i[XSENS_COUNT];
+
+extern float xsens_accel_x[XSENS_COUNT];
+extern float xsens_accel_y[XSENS_COUNT];
+extern float xsens_accel_z[XSENS_COUNT];
+extern float xsens_mag_x[XSENS_COUNT];
+extern float xsens_mag_y[XSENS_COUNT];
+extern float xsens_mag_z[XSENS_COUNT];
+extern float xsens_gyro_x[XSENS_COUNT];
+extern float xsens_gyro_y[XSENS_COUNT];
+extern float xsens_gyro_z[XSENS_COUNT];
+extern float xsens_mag_heading[XSENS_COUNT];
+
+extern uint16_t xsens_time_stamp[XSENS_COUNT];
+
+#define PERIODIC_SEND_IMU_GYRO() DOWNLINK_SEND_IMU_GYRO (\
+  &xsens_gyro_x, &xsens_gyro_y, &xsens_gyro_z \
+)
+
+#define PERIODIC_SEND_IMU_ACCEL() DOWNLINK_SEND_IMU_ACCEL (\
+  &xsens_accel_x, &xsens_accel_y, &xsens_accel_z \
+)
+
+#define PERIODIC_SEND_IMU_MAG() DOWNLINK_SEND_IMU_MAG (\
+  &xsens_mag_x, &xsens_mag_y, &xsens_mag_z \
+)
+
+/*
+#define PERIODIC_SEND_XSENS_DATA() DOWNLINK_SEND_XSENS_DATA (\
+  &whirly_timestamp, \
+  xsens_accel_x, xsens_accel_y, xsens_accel_z, \
+  xsens_mag_x, xsens_mag_y, xsens_mag_z, \
+  xsens_gyro_x, xsens_gyro_y, xsens_gyro_z, \
+  xsens_time_stamp, \
+  xsens_accel_x + 1, xsens_accel_y + 1, xsens_accel_z + 1, \
+  xsens_mag_x + 1, xsens_mag_y + 1, xsens_mag_z + 1, \
+  xsens_gyro_x + 1, xsens_gyro_y + 1, xsens_gyro_z + 1, \
+  xsens_time_stamp + 1 \
+)
+*/
+
+#include "std.h"
+
+void xsens_init(void);
+void xsens_parse_msg(uint8_t xsens_id);
+void xsens_event_task(void);
+void xsens_periodic_task(void);
+
+#endif