diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
index 09f465142e..5503a1aede 100644
--- a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
+++ b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
@@ -2,9 +2,10 @@
# XSens Mti-G
-#
+#
#
-#
+#
+#
@@ -13,35 +14,19 @@
ifeq ($(TARGET), ap)
-#
-#
-#
-#
-#
-#
-#
-#
-#
-#
-#
-#
-#
-#
-#
-#
-
-# ImuEvent -> XSensEvent
-ap.CFLAGS += -DUSE_AHRS
-ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_xsens.h\"
+ap.CFLAGS += -DUSE_INS
# AHRS Results
-ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_xsens.h\"
ap.CFLAGS += -DINS_MODULE_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
+
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 += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD)
ap.CFLAGS += -DUSE_GPS_XSENS
ap.CFLAGS += -DUSE_GPS_XSENS_RAW_DATA
ap.CFLAGS += -DGPS_NB_CHANNELS=16
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index c76f12b1d5..23e0ee6192 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -188,6 +188,10 @@ void init_ap( void ) {
ahrs_init();
#endif
+#if USE_INS
+ ins_init();
+#endif
+
/************* Links initialization ***************/
#if defined MCU_SPI_LINK
link_mcu_init();
@@ -493,11 +497,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 ) {
@@ -532,6 +534,10 @@ void sensors_task( void ) {
ahrs_timeout_counter ++;
#endif // USE_AHRS
#endif // USE_IMU
+
+#if USE_INS
+ ins_periodic_task();
+#endif
}
@@ -589,6 +595,10 @@ void event_task_ap( void ) {
ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
#endif
+#if USE_INS
+ InsEvent(NULL);
+#endif
+
#if USE_GPS
GpsEvent(on_gps_solution);
#endif /** USE_GPS */
@@ -624,7 +634,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
diff --git a/sw/airborne/modules/ins/ins_module.h b/sw/airborne/modules/ins/ins_module.h
index 194dcb5f89..5e8f0e77bc 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
@@ -97,7 +96,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 8da99364ff..232efaefa6 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"
@@ -72,25 +70,6 @@ float ins_pitch_neutral;
float ins_roll_neutral;
-void ahrs_init(void)
-{
- ins_init();
-}
-
-
-#include "subsystems/imu.h"
-
-void imu_init(void)
-{
- ins_init();
-}
-
-void imu_periodic(void)
-{
- ins_periodic_task();
-}
-
-
//////////////////////////////////////////////////////////////////////////////////////////
//
// XSens Specific
@@ -242,65 +221,68 @@ void ins_init( void ) {
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());
}
#include "estimator.h"
-void handle_ins_msg( void) {
+void handle_ins_msg(void) {
// Send to Estimator (Control)
@@ -611,6 +593,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..f573916edf 100644
--- a/sw/airborne/modules/ins/ins_xsens.h
+++ b/sw/airborne/modules/ins/ins_xsens.h
@@ -33,9 +33,6 @@
#include "ins_module.h"
-#include "subsystems/ahrs.h"
-
-
extern int8_t xsens_hour;
extern int8_t xsens_min;
extern int8_t xsens_sec;
@@ -46,8 +43,7 @@ extern int8_t xsens_day;
extern uint8_t xsens_msg_status;
extern uint16_t xsens_time_stamp;
-#define AhrsEvent(_ahrs_handler) { \
- LED_TOGGLE(3); \
+#define InsEvent(_ins_handler) { \
InsEventCheckAndHandle(handle_ins_msg()) \
}