mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
[gps] fix skytraq binary parser: correct scaling and signed values, thx for reporting Gerard
while here clean up a little hopefully finally closes #167
This commit is contained in:
Felix Ruess
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user