From 528b6f40ec59d6762247077e4f3b3920aeed0188 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 18 Feb 2013 20:45:15 +0100 Subject: [PATCH] [gps] fix skytraq binary parser: correct scaling and signed values, thx for reporting Gerard while here clean up a little hopefully finally closes #167 --- sw/airborne/subsystems/gps/gps_skytraq.c | 218 +++++++++++++---------- sw/airborne/subsystems/gps/gps_skytraq.h | 56 ++---- 2 files changed, 141 insertions(+), 133 deletions(-) diff --git a/sw/airborne/subsystems/gps/gps_skytraq.c b/sw/airborne/subsystems/gps/gps_skytraq.c index 89b96e84ea..35b19ae48f 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.c +++ b/sw/airborne/subsystems/gps/gps_skytraq.c @@ -46,29 +46,58 @@ struct GpsSkytraq gps_skytraq; #define SKYTRAQ_FIX_3D 0x02 #define SKYTRAQ_FIX_3D_DGPS 0x03 + +#define SKYTRAQ_SYNC1 0xA0 +#define SKYTRAQ_SYNC2 0xA1 + +#define SKYTRAQ_SYNC3 0x0D +#define SKYTRAQ_SYNC4 0x0A + + +static inline uint16_t bswap16(uint16_t a) { + return (a<<8)|(a>>8); +} + +#define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload) (uint8_t) (*((uint8_t*)_payload+2-2)) +#define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload) (uint8_t) (*((uint8_t*)_payload+3-2)) + +#define SKYTRAQ_NAVIGATION_DATA_WEEK(_payload) bswap16(*(uint16_t*)&_payload[4-2]) +#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload) __builtin_bswap32(*(uint32_t*)&_payload[6-2]) + +#define SKYTRAQ_NAVIGATION_DATA_LAT(_payload) (int32_t)__builtin_bswap32(*( int32_t*)&_payload[10-2]) +#define SKYTRAQ_NAVIGATION_DATA_LON(_payload) (int32_t)__builtin_bswap32(*( int32_t*)&_payload[14-2]) + +#define SKYTRAQ_NAVIGATION_DATA_AEL(_payload) __builtin_bswap32(*(uint32_t*)&_payload[18-2]) +#define SKYTRAQ_NAVIGATION_DATA_ASL(_payload) __builtin_bswap32(*(uint32_t*)&_payload[22-2]) + +#define SKYTRAQ_NAVIGATION_DATA_GDOP(_payload) bswap16(*(uint16_t*)&_payload[26-2]) +#define SKYTRAQ_NAVIGATION_DATA_PDOP(_payload) bswap16(*(uint16_t*)&_payload[28-2]) +#define SKYTRAQ_NAVIGATION_DATA_HDOP(_payload) bswap16(*(uint16_t*)&_payload[30-2]) +#define SKYTRAQ_NAVIGATION_DATA_VDOP(_payload) bswap16(*(uint16_t*)&_payload[32-2]) +#define SKYTRAQ_NAVIGATION_DATA_TDOP(_payload) bswap16(*(uint16_t*)&_payload[34-2]) + +#define SKYTRAQ_NAVIGATION_DATA_ECEFX(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[36-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFY(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[40-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFZ(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[44-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFVX(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[48-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFVY(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[52-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFVZ(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[56-2]) + + // distance in cm (10km dist max any direction) #define MAX_DISTANCE 1000000 -//#include "my_debug_servo.h" - -struct LtpDef_i ref_ltp; - -static int distance_too_great( struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos ); +static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos); void gps_impl_init(void) { gps_skytraq.status = UNINIT; - - //DEBUG_SERVO1_INIT(); - } void gps_skytraq_read_message(void) { - //DEBUG_S1_ON(); - if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { gps.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(gps_skytraq.msg_buf); gps.ecef_pos.y = SKYTRAQ_NAVIGATION_DATA_ECEFY(gps_skytraq.msg_buf); @@ -78,24 +107,24 @@ void gps_skytraq_read_message(void) { gps.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(gps_skytraq.msg_buf); gps.lla_pos.lat = RadOfDeg(SKYTRAQ_NAVIGATION_DATA_LAT(gps_skytraq.msg_buf)); gps.lla_pos.lon = RadOfDeg(SKYTRAQ_NAVIGATION_DATA_LON(gps_skytraq.msg_buf)); - gps.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(gps_skytraq.msg_buf)/10; - gps.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf)/10; + gps.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(gps_skytraq.msg_buf)*10; + gps.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf)*10; // pacc; // sacc; - // gps.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf); + gps.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf); gps.num_sv = SKYTRAQ_NAVIGATION_DATA_NumSV(gps_skytraq.msg_buf); - gps.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf)/10; + gps.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf)*10; switch (SKYTRAQ_NAVIGATION_DATA_FixMode(gps_skytraq.msg_buf)) { - case SKYTRAQ_FIX_3D_DGPS: - case SKYTRAQ_FIX_3D: - gps.fix = GPS_FIX_3D; - break; - case SKYTRAQ_FIX_2D: - gps.fix = GPS_FIX_2D; - break; - default: - gps.fix = GPS_FIX_NONE; + case SKYTRAQ_FIX_3D_DGPS: + case SKYTRAQ_FIX_3D: + gps.fix = GPS_FIX_3D; + break; + case SKYTRAQ_FIX_2D: + gps.fix = GPS_FIX_2D; + break; + default: + gps.fix = GPS_FIX_NONE; } #if GPS_USE_LATLONG @@ -114,30 +143,27 @@ void gps_skytraq_read_message(void) { gps.utm_pos.zone = nav_utm_zone0; #endif - if ( gps.fix == GPS_FIX_3D ) { - if ( distance_too_great( &ref_ltp.ecef, &gps.ecef_pos ) ) { + if (gps.fix == GPS_FIX_3D) { + if ( distance_too_great(&gps_skytraq.ref_ltp.ecef, &gps.ecef_pos)) { // just grab current ecef_pos as reference. - ltp_def_from_ecef_i( &ref_ltp, &gps.ecef_pos ); + ltp_def_from_ecef_i(&gps_skytraq.ref_ltp, &gps.ecef_pos); } // convert ecef velocity vector to NED vector. - ned_of_ecef_vect_i( &gps.ned_vel, &ref_ltp, &gps.ecef_vel ); + ned_of_ecef_vect_i(&gps.ned_vel, &gps_skytraq.ref_ltp, &gps.ecef_vel); // ground course in radians gps.course = ( M_PI_4 + atan2( -gps.ned_vel.y, gps.ned_vel.x )) * 1e7; // GT: gps.cacc = ... ? what should course accuracy be? // ground speed - gps.gspeed = sqrt( gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y ); - gps.speed_3d = sqrt( gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y + gps.ned_vel.z * gps.ned_vel.z ); + gps.gspeed = sqrt(gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y); + gps.speed_3d = sqrt(gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y + gps.ned_vel.z * gps.ned_vel.z); // vertical speed (climb) // solved by gps.ned.z? } - - //DEBUG_S2_TOGGLE(); - #ifdef GPS_LED if (gps.fix == GPS_FIX_3D) { LED_ON(GPS_LED); @@ -148,69 +174,69 @@ void gps_skytraq_read_message(void) { #endif } - //DEBUG_S1_OFF(); } void gps_skytraq_parse(uint8_t c) { - if (gps_skytraq.status < GOT_PAYLOAD) + if (gps_skytraq.status < GOT_PAYLOAD) { gps_skytraq.checksum ^= c; + } switch (gps_skytraq.status) { - case UNINIT: - if (c == SKYTRAQ_SYNC1) - gps_skytraq.status = GOT_SYNC1; - break; - case GOT_SYNC1: - if (c != SKYTRAQ_SYNC2) { - gps_skytraq.error_last = GPS_SKYTRAQ_ERR_OUT_OF_SYNC; + case UNINIT: + if (c == SKYTRAQ_SYNC1) + gps_skytraq.status = GOT_SYNC1; + break; + case GOT_SYNC1: + if (c != SKYTRAQ_SYNC2) { + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_OUT_OF_SYNC; + goto error; + } + gps_skytraq.status = GOT_SYNC2; + break; + case GOT_SYNC2: + gps_skytraq.len = c<<8; + gps_skytraq.status = GOT_LEN1; + break; + case GOT_LEN1: + gps_skytraq.len += c; + gps_skytraq.status = GOT_LEN2; + if (gps_skytraq.len > GPS_SKYTRAQ_MAX_PAYLOAD) { + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_MSG_TOO_LONG; + goto error; + } + break; + case GOT_LEN2: + gps_skytraq.msg_id = c; + gps_skytraq.msg_idx = 0; + gps_skytraq.checksum = c; + gps_skytraq.status = GOT_ID; + break; + case GOT_ID: + gps_skytraq.msg_buf[gps_skytraq.msg_idx] = c; + gps_skytraq.msg_idx++; + if (gps_skytraq.msg_idx >= gps_skytraq.len-1) { + gps_skytraq.status = GOT_PAYLOAD; + } + break; + case GOT_PAYLOAD: + if (c != gps_skytraq.checksum) { + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_CHECKSUM; + goto error; + } + gps_skytraq.status = GOT_CHECKSUM; + break; + case GOT_CHECKSUM: + if (c != SKYTRAQ_SYNC3) { + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_OUT_OF_SYNC; + goto error; + } + gps_skytraq.status = GOT_SYNC3; + break; + case GOT_SYNC3: + gps_skytraq.msg_available = TRUE; + goto restart; + default: + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_UNEXPECTED; goto error; - } - gps_skytraq.status = GOT_SYNC2; - break; - case GOT_SYNC2: - gps_skytraq.len = c<<8; - gps_skytraq.status = GOT_LEN1; - break; - case GOT_LEN1: - gps_skytraq.len += c; - gps_skytraq.status = GOT_LEN2; - if (gps_skytraq.len > GPS_SKYTRAQ_MAX_PAYLOAD) { - gps_skytraq.error_last = GPS_SKYTRAQ_ERR_MSG_TOO_LONG; - goto error; - } - break; - case GOT_LEN2: - gps_skytraq.msg_id = c; - gps_skytraq.msg_idx = 0; - gps_skytraq.checksum = c; - gps_skytraq.status = GOT_ID; - break; - case GOT_ID: - gps_skytraq.msg_buf[gps_skytraq.msg_idx] = c; - gps_skytraq.msg_idx++; - if (gps_skytraq.msg_idx >= gps_skytraq.len-1) { - gps_skytraq.status = GOT_PAYLOAD; - } - break; - case GOT_PAYLOAD: - if (c != gps_skytraq.checksum) { - gps_skytraq.error_last = GPS_SKYTRAQ_ERR_CHECKSUM; - goto error; - } - gps_skytraq.status = GOT_CHECKSUM; - break; - case GOT_CHECKSUM: - if (c != SKYTRAQ_SYNC3) { - gps_skytraq.error_last = GPS_SKYTRAQ_ERR_OUT_OF_SYNC; - goto error; - } - gps_skytraq.status = GOT_SYNC3; - break; - case GOT_SYNC3: - gps_skytraq.msg_available = TRUE; - goto restart; - default: - gps_skytraq.error_last = GPS_SKYTRAQ_ERR_UNEXPECTED; - goto error; } return; error: @@ -220,13 +246,19 @@ void gps_skytraq_parse(uint8_t c) { return; } -static int distance_too_great( struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos ) { +static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos) { int32_t xdiff = abs(ecef_ref->x - ecef_pos->x); - if ( xdiff > MAX_DISTANCE ) return TRUE; + if (xdiff > MAX_DISTANCE) { + return TRUE; + } int32_t ydiff = abs(ecef_ref->y - ecef_pos->y); - if ( ydiff > MAX_DISTANCE ) return TRUE; + if (ydiff > MAX_DISTANCE) { + return TRUE; + } int32_t zdiff = abs(ecef_ref->z - ecef_pos->z); - if ( zdiff > MAX_DISTANCE ) return TRUE; + if (zdiff > MAX_DISTANCE) { + return TRUE; + } return FALSE; } diff --git a/sw/airborne/subsystems/gps/gps_skytraq.h b/sw/airborne/subsystems/gps/gps_skytraq.h index d7c24bbd3a..a9ab5762e4 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.h +++ b/sw/airborne/subsystems/gps/gps_skytraq.h @@ -24,56 +24,32 @@ #include "mcu_periph/uart.h" -#define SKYTRAQ_SYNC1 0xA0 -#define SKYTRAQ_SYNC2 0xA1 - -#define SKYTRAQ_SYNC3 0x0D -#define SKYTRAQ_SYNC4 0x0A - - #define SKYTRAQ_ID_NAVIGATION_DATA 0XA8 -#define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload) (uint8_t) (*((uint8_t*)_payload+2-2)) -#define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload) (uint8_t) (*((uint8_t*)_payload+3-2)) - -//#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload) (uint32_t)(_payload[7] + (((uint32_t)_payload[6])<<8) + (((uint32_t)_payload[5])<<16) + (((uint32_t)_payload[4])<<24)) -#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload) __builtin_bswap32(*(uint32_t*)&_payload[ 6-2]) -#define SKYTRAQ_NAVIGATION_DATA_LAT(_payload) __builtin_bswap32(*( int32_t*)&_payload[10-2]) -#define SKYTRAQ_NAVIGATION_DATA_LON(_payload) __builtin_bswap32(*( int32_t*)&_payload[14-2]) -#define SKYTRAQ_NAVIGATION_DATA_AEL(_payload) __builtin_bswap32(*(uint32_t*)&_payload[18-2]) -#define SKYTRAQ_NAVIGATION_DATA_ASL(_payload) __builtin_bswap32(*(uint32_t*)&_payload[22-2]) -//#define SKYTRAQ_NAVIGATION_DATA_GDOP(_payload) __builtin_bswap16(*(uint16_t*)&_payload[26-2]) -//#define SKYTRAQ_NAVIGATION_DATA_PDOP(_payload) __builtin_bswap(*(uint16_t*)&_payload[28-2]) - -#define SKYTRAQ_NAVIGATION_DATA_ECEFX(_payload) __builtin_bswap32(*( int32_t*)&_payload[36-2]) -#define SKYTRAQ_NAVIGATION_DATA_ECEFY(_payload) __builtin_bswap32(*( int32_t*)&_payload[40-2]) -#define SKYTRAQ_NAVIGATION_DATA_ECEFZ(_payload) __builtin_bswap32(*( int32_t*)&_payload[44-2]) -#define SKYTRAQ_NAVIGATION_DATA_ECEFVX(_payload) __builtin_bswap32(*( int32_t*)&_payload[48-2]) -#define SKYTRAQ_NAVIGATION_DATA_ECEFVY(_payload) __builtin_bswap32(*( int32_t*)&_payload[52-2]) -#define SKYTRAQ_NAVIGATION_DATA_ECEFVZ(_payload) __builtin_bswap32(*( int32_t*)&_payload[56-2]) - - - /* last error type */ -#define GPS_SKYTRAQ_ERR_NONE 0 -#define GPS_SKYTRAQ_ERR_OVERRUN 1 -#define GPS_SKYTRAQ_ERR_MSG_TOO_LONG 2 -#define GPS_SKYTRAQ_ERR_CHECKSUM 3 -#define GPS_SKYTRAQ_ERR_OUT_OF_SYNC 4 -#define GPS_SKYTRAQ_ERR_UNEXPECTED 5 +enum GpsSkytraqError { + GPS_SKYTRAQ_ERR_NONE = 0, + GPS_SKYTRAQ_ERR_OVERRUN, + GPS_SKYTRAQ_ERR_MSG_TOO_LONG, + GPS_SKYTRAQ_ERR_CHECKSUM, + GPS_SKYTRAQ_ERR_OUT_OF_SYNC, + GPS_SKYTRAQ_ERR_UNEXPECTED +}; #define GPS_SKYTRAQ_MAX_PAYLOAD 255 struct GpsSkytraq { - uint8_t msg_buf[GPS_SKYTRAQ_MAX_PAYLOAD] __attribute__ ((aligned)); + uint8_t msg_buf[GPS_SKYTRAQ_MAX_PAYLOAD]; bool_t msg_available; uint8_t msg_id; - uint8_t status; + uint8_t status; uint16_t len; - uint8_t msg_idx; - uint8_t checksum; - uint8_t error_cnt; - uint8_t error_last; + uint8_t msg_idx; + uint8_t checksum; + uint8_t error_cnt; + enum GpsSkytraqError error_last; + + struct LtpDef_i ref_ltp; }; extern struct GpsSkytraq gps_skytraq;