From eac42b1c90132dc12ce9a0bb5fe7498b7cf55909 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Wed, 18 Aug 2010 13:19:11 +0000 Subject: [PATCH] Flying Fixed-Wings with XSens --- sw/airborne/gps_xsens.c | 5 +++ sw/airborne/gps_xsens.h | 5 ++- sw/airborne/modules/ins/ins_xsens.c | 63 ++++++++++++++++++++++++++--- 3 files changed, 66 insertions(+), 7 deletions(-) diff --git a/sw/airborne/gps_xsens.c b/sw/airborne/gps_xsens.c index 5b890bb9d5..9d7ce28301 100644 --- a/sw/airborne/gps_xsens.c +++ b/sw/airborne/gps_xsens.c @@ -54,6 +54,11 @@ 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; diff --git a/sw/airborne/gps_xsens.h b/sw/airborne/gps_xsens.h index a6361296fb..95878f56d4 100644 --- a/sw/airborne/gps_xsens.h +++ b/sw/airborne/gps_xsens.h @@ -35,7 +35,10 @@ extern uint16_t gps_reset; #define GPS_NB_CHANNELS 16 -#define GpsFixValid() (gps_mode == 3) +extern void reset_gps_watchdog(void); + + +#define GpsFixValid() (gps_mode > 0) #define gps_xsens_Reset(_val) { \ gps_reset = _val; \ diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index 42b18f61e4..4e20fbe40d 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -90,11 +90,56 @@ volatile uint8_t ins_msg_received; #define XSENS_MAX_PAYLOAD 254 uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD]; -/* output mode : calibrated, orientation, position, velocity, status */ +/* output mode : calibrated, orientation, position, velocity, status + * ----------- + * + * bit 0 temp + * bit 1 calibrated + * bit 2 orentation + * bit 3 aux + * + * bit 4 position + * bit 5 velocity + * bit 6-7 Reserved + * + * bit 8-10 Reserved + * bit 11 status + * + * bit 12 GPS PVT+baro + * bit 13 Reserved + * bit 14 Raw + * bit 15 Reserved + */ + #ifndef XSENS_OUTPUT_MODE -#define XSENS_OUTPUT_MODE 0x0836 +#define XSENS_OUTPUT_MODE 0x1836 #endif -/* output settings : timestamp, euler, acc, rate, mag, float, no aux, lla, m/s, NED */ +/* output settings : timestamp, euler, acc, rate, mag, float, no aux, lla, m/s, NED + * ----------- + * + * bit 01 0=none, 1=sample counter, 2=utc, 3=sample+utc + * bit 23 0=quaternion, 1=euler, 2=DCM + * + * bit 4 1=disable acc output + * bit 5 1=disable gyro output + * bit 6 1=disable magneto output + * bit 7 Reserved + * + * bit 89 0=float, 1=fixedpoint12.20, 2=fixedpoint16.32 + * bit 10 1=disable aux analog 1 + * bit 11 1=disable aux analog 2 + * + * bit 12-15 0-only: 14-16 WGS84 + * + * bit 16-19 0-only: 16-18 m/s XYZ + * + * bit 20-23 Reserved + * + * bit 24-27 Reseverd + * + * bit 28-30 Reseverd + * bit 31 0=X-North-Z-Up, 1=North-East-Down + */ #ifndef XSENS_OUTPUT_SETTINGS #define XSENS_OUTPUT_SETTINGS 0x80000C05 #endif @@ -161,6 +206,7 @@ void parse_ins_msg( void ) { #ifdef USE_GPS_XSENS else if (xsens_id == XSENS_GPSStatus_ID) { gps_nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf); + gps_numSV = gps_nb_channels; uint8_t i; for(i = 0; i < Min(gps_nb_channels, GPS_NB_CHANNELS); i++) { uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i); @@ -184,7 +230,8 @@ void parse_ins_msg( void ) { if (XSENS_MASK_RAWGPS(xsens_output_mode)) { #ifdef USE_GPS_XSENS gps_week = 0; // FIXME - gps_itow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset); + gps_itow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10; +#ifdef USE_GPS_XSENS_RAW_DATA gps_lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset); gps_lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset); /* Set the real UTM zone */ @@ -201,8 +248,10 @@ void parse_ins_msg( void ) { ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.; ins_vz = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.; gps_climb = -XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 10; - gps_Pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset); - gps_Sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset); +#endif + gps_Pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100; + gps_Sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100; + gps_PDOP = 5; #endif offset += XSENS_DATA_RAWGPS_LENGTH; } @@ -290,6 +339,7 @@ void parse_ins_msg( void ) { #ifndef USE_VFILTER gps_alt = ins_z * 100; #endif + reset_gps_watchdog(); #endif offset += XSENS_DATA_Position_LENGTH; } @@ -309,6 +359,7 @@ void parse_ins_msg( void ) { xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf,offset); #ifdef USE_GPS_XSENS if (bit_is_set(xsens_msg_status,2)) gps_mode = 0x03; // gps fix + else if (bit_is_set(xsens_msg_status,1)) gps_mode = 0x01; // efk valid else gps_mode = 0; #endif offset += XSENS_DATA_Status_LENGTH;