diff --git a/conf/autopilot/csc_ap.makefile b/conf/autopilot/csc_ap.makefile
index bd2abf29f9..d03484cb61 100644
--- a/conf/autopilot/csc_ap.makefile
+++ b/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
diff --git a/conf/telemetry/csc_ap.xml b/conf/telemetry/csc_ap.xml
index 41e23c191b..efb01431bc 100644
--- a/conf/telemetry/csc_ap.xml
+++ b/conf/telemetry/csc_ap.xml
@@ -4,15 +4,18 @@
-
-
-
+
+
+
+
+
+
diff --git a/sw/airborne/csc/csc_ap_main.c b/sw/airborne/csc/csc_ap_main.c
index 9ea1c9177d..c23a252c4d 100644
--- a/sw/airborne/csc/csc_ap_main.c
+++ b/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) {
diff --git a/sw/airborne/csc/csc_xsens.c b/sw/airborne/csc/csc_xsens.c
new file mode 100644
index 0000000000..b644d7f271
--- /dev/null
+++ b/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
+
+#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;
+}
diff --git a/sw/airborne/csc/csc_xsens.h b/sw/airborne/csc/csc_xsens.h
new file mode 100644
index 0000000000..8cc2bbd6c0
--- /dev/null
+++ b/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