diff --git a/conf/airframes/TU_Delft/yapa_xsens.xml b/conf/airframes/TU_Delft/yapa_xsens.xml
index 84d95e43c8..10abe2b9f1 100644
--- a/conf/airframes/TU_Delft/yapa_xsens.xml
+++ b/conf/airframes/TU_Delft/yapa_xsens.xml
@@ -1,6 +1,6 @@
-
@@ -51,7 +51,7 @@
+ -->
+
+
@@ -218,6 +220,8 @@
-
+
+
+
diff --git a/conf/autopilot/subsystems/fixedwing/imu_booz.makefile b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
index e6ad633f17..c6ee27d0c1 100644
--- a/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
+++ b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
@@ -52,7 +52,6 @@ imu_srcs += $(SRC_ARCH)/peripherals/max1168_arch.c
#ifeq ($(ARCH), lpc21)
imu_CFLAGS += -DSSP_VIC_SLOT=9
imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
-#FIXME ms2100 not used on this imu
#else ifeq ($(ARCH), stm32)
#imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
#imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
diff --git a/conf/modules/ins_xsens_MTiG_fixedwing.xml b/conf/modules/ins_xsens_MTiG_fixedwing.xml
index 4990c68b8c..dcd3c87cb4 100644
--- a/conf/modules/ins_xsens_MTiG_fixedwing.xml
+++ b/conf/modules/ins_xsens_MTiG_fixedwing.xml
@@ -13,10 +13,13 @@
-
+
+
+
+
diff --git a/conf/xsens_MTi-G.xml b/conf/xsens_MTi-G.xml
index 4db7955d4c..721c801aef 100644
--- a/conf/xsens_MTi-G.xml
+++ b/conf/xsens_MTi-G.xml
@@ -40,6 +40,11 @@
+
+
+
+
+
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index 248893f693..8cb3248fb8 100644
--- a/sw/airborne/ap_downlink.h
+++ b/sw/airborne/ap_downlink.h
@@ -191,7 +191,7 @@
int16_t climb = -gps.ned_vel.z; \
int16_t course = DegOfRad(gps.course / 10); \
DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \
- if (i == gps.nb_channels) i = 0; \
+ if (i >= gps.nb_channels) i = 0; \
if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { \
DOWNLINK_SEND_SVINFO(DefaultChannel, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \
last_cnos[i] = gps.svinfos[i].cno; \
diff --git a/sw/airborne/gps_xsens.c b/sw/airborne/gps_xsens.c
deleted file mode 100644
index 077d2353af..0000000000
--- a/sw/airborne/gps_xsens.c
+++ /dev/null
@@ -1,74 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2005 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 gps_xsens.c
- * \brief GPS hardware for Xsens MTi-G
- *
- */
-
-#include
-
-#include "gps.h"
-#include "sys_time.h"
-#include "generated/airframe.h"
-
-#include "mcu_periph/uart.h"
-#include "messages.h"
-#include "downlink.h"
-
-uint8_t gps_mode;
-uint16_t gps_week;
-uint32_t gps_itow;
-int32_t gps_alt;
-uint16_t gps_gspeed;
-int16_t gps_climb;
-int16_t gps_course;
-int32_t gps_utm_east, gps_utm_north;
-uint8_t gps_utm_zone;
-int32_t gps_lat, gps_lon;
-uint16_t gps_PDOP;
-uint32_t gps_Pacc, gps_Sacc;
-uint8_t gps_numSV;
-uint8_t gps_configuring;
-
-uint16_t gps_reset;
-
-void reset_gps_watchdog(void)
-{
- last_gps_msg_t = cpu_time_sec;
-}
-
-volatile bool_t gps_msg_received;
-bool_t gps_pos_available;
-uint8_t gps_nb_ovrn;
-
-uint8_t gps_nb_channels;
-
-struct svinfo gps_svinfos[GPS_NB_CHANNELS];
-
-
-void gps_init( void ) {}
-void gps_configure( void ) {}
-void parse_gps_msg( void ) {}
-void gps_configure_uart( void ) {}
diff --git a/sw/airborne/gps_xsens.h b/sw/airborne/gps_xsens.h
deleted file mode 100644
index 2395793e5a..0000000000
--- a/sw/airborne/gps_xsens.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * Paparazzi autopilot $Id: gps_ubx.h 4659 2010-03-10 16:55:04Z mmm $
- *
- * Copyright (C) 2004-2006 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 gps_xsens.h
- * \brief XSens GPS
- *
-*/
-
-
-#ifndef XSENS_GPS_H
-#define XSENS_GPS_H
-
-extern uint16_t gps_reset;
-
-#define GPS_NB_CHANNELS 16
-
-extern void reset_gps_watchdog(void);
-
-
-#define GpsFixValid() (gps_mode > 0)
-
-#define gps_xsens_Reset(_val) { \
- gps_reset = _val; \
-}
-
-
-#endif
diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c
index 3929155ec4..7f6453795b 100644
--- a/sw/airborne/modules/ins/ins_chimu_uart.c
+++ b/sw/airborne/modules/ins/ins_chimu_uart.c
@@ -34,22 +34,22 @@ INS_FORMAT ins_pitch_neutral;
volatile uint8_t new_ins_attitude;
-void ins_init( void )
+void ins_init( void )
{
uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
-
+
CHIMU_Checksum(rate,12);
new_ins_attitude = 0;
-
+
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
-
- CHIMU_Init(&CHIMU_DATA);
-
+
+ CHIMU_Init(&CHIMU_DATA);
+
// Quat Filter
for (int i=0;i<7;i++)
{
@@ -68,7 +68,7 @@ void ins_init( void )
{
InsUartSend1(rate[i]);
}
-
+
}
void parse_ins_msg( void )
@@ -76,7 +76,7 @@ void parse_ins_msg( void )
while (InsLink(ChAvailable()))
{
uint8_t ch = InsLink(Getch());
-
+
if (CHIMU_Parse(ch, 0, &CHIMU_DATA))
{
if(CHIMU_DATA.m_MsgID==0x03)
@@ -87,10 +87,10 @@ void parse_ins_msg( void )
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
-
+
EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
//EstimatorSetRate(ins_p,ins_q);
-
+
DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
}
@@ -100,7 +100,7 @@ void parse_ins_msg( void )
//Frequency defined in conf *.xml
-void ins_periodic_task( void )
+void ins_periodic_task( void )
{
// Downlink Send
}
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index adb9d0513e..c26f0d0947 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -68,6 +68,8 @@ INS_FORMAT ins_mz;
float ins_pitch_neutral;
float ins_roll_neutral;
+volatile uint8_t new_ins_attitude;
+
//////////////////////////////////////////////////////////////////////////////////////////
//
// XSens Specific
@@ -177,9 +179,6 @@ int8_t xsens_day;
float xsens_lat;
float xsens_lon;
-int8_t xsens_gps_nr_channels = 16;
-
-
static uint8_t xsens_id;
static uint8_t xsens_status;
static uint8_t xsens_len;
@@ -205,7 +204,16 @@ void ins_init( void ) {
XSENS_GoToConfig();
XSENS_SetOutputMode(xsens_output_mode);
XSENS_SetOutputSettings(xsens_output_settings);
+
+ uint8_t baud = 1;
+ XSENS_SetBaudrate(baud);
+ // Give pulses on SyncOut
+ XSENS_SetSyncOutSettings(0,0x0002);
+ // 1 pulse every 100 samples
+ // XSENS_SetSyncOutSettings(1,100);
//XSENS_GoToMeasurment();
+
+ gps.nb_channels = 0;
}
void ins_periodic_task( void ) {
@@ -243,16 +251,22 @@ void parse_ins_msg( void ) {
}
#ifdef USE_GPS_XSENS
else if (xsens_id == XSENS_GPSStatus_ID) {
- xsens_gps_nr_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
- gps.num_sv = xsens_gps_nr_channels;
+ gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
+ gps.num_sv = 0;
+
uint8_t i;
- for(i = 0; i < Min(xsens_gps_nr_channels, xsens_gps_nr_channels); i++) {
+ // Do not write outside buffer
+ for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i);
- if (ch > xsens_gps_nr_channels) continue;
+ if (ch > gps.nb_channels) continue;
gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
+ if (gps.svinfos[ch].flags > 0)
+ {
+ gps.num_sv++;
+ }
}
}
#endif
@@ -267,15 +281,18 @@ void parse_ins_msg( void ) {
}
if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
#if defined(USE_GPS_XSENS_RAW_DATA) && defined(USE_GPS_XSENS)
+ LED_TOGGLE(3);
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));
+
+
/* Set the real UTM zone */
gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
- lla_f.lat = (float) gps.lla_pos.lat * 1e7;
- lla_f.lon = (float) gps.lla_pos.lat * 1e7;
+ lla_f.lat = (float) gps.lla_pos.lat / 1e7;
+ lla_f.lon = (float) gps.lla_pos.lat / 1e7;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
@@ -348,6 +365,7 @@ void parse_ins_msg( void ) {
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
offset += XSENS_DATA_Matrix_LENGTH;
}
+ new_ins_attitude = 1;
}
if (XSENS_MASK_Auxiliary(xsens_output_mode)) {
uint8_t l = 0;
diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c
index dfffde6094..cc3965c14d 100644
--- a/sw/airborne/modules/sensors/mag_hmc5843.c
+++ b/sw/airborne/modules/sensors/mag_hmc5843.c
@@ -46,7 +46,7 @@ void hmc5843_module_periodic ( void )
mag_x = hmc5843.data.value[0];
mag_y = hmc5843.data.value[1];
mag_z = hmc5843.data.value[2];
- RunOnceEvery(1,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
+ RunOnceEvery(30,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
}
void hmc5843_module_event( void )