From d9f78faecaf47b9b02bb666ea6cf5426ec7dd5d4 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Wed, 4 May 2011 13:06:12 +0200 Subject: [PATCH] XSens -> 230k4: otherwise half the data is lost. Fixed GPS_NB_CHANNELS issue with new GPS --- conf/airframes/TU_Delft/yapa_xsens.xml | 14 ++-- .../subsystems/fixedwing/imu_booz.makefile | 1 - conf/modules/ins_xsens_MTiG_fixedwing.xml | 5 +- conf/xsens_MTi-G.xml | 5 ++ sw/airborne/ap_downlink.h | 2 +- sw/airborne/gps_xsens.c | 74 ------------------- sw/airborne/gps_xsens.h | 48 ------------ sw/airborne/modules/ins/ins_chimu_uart.c | 22 +++--- sw/airborne/modules/ins/ins_xsens.c | 36 ++++++--- sw/airborne/modules/sensors/mag_hmc5843.c | 2 +- 10 files changed, 58 insertions(+), 151 deletions(-) delete mode 100644 sw/airborne/gps_xsens.c delete mode 100644 sw/airborne/gps_xsens.h 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 )