From ee4919dd771b00e2a2dd1083c9528efa7baab50f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 29 Jun 2009 13:54:00 -0700 Subject: [PATCH] Convert GPS to SiRF binary protocol. This switches the GPS unit from NMEA to SiRF protocol at startup and then parses the binary data. The binary data uses a different encoding of lat/lon than the NMEA strings, and so the telemetry and eeprom data formats change with this switch. Signed-off-by: Keith Packard --- src/ao.h | 24 +-- src/ao_gps.c | 417 ++++++++++++++++++++++++-------------------- src/ao_gps_print.c | 44 +++-- src/ao_gps_report.c | 8 +- 4 files changed, 272 insertions(+), 221 deletions(-) diff --git a/src/ao.h b/src/ao.h index 6836e010..d4df1595 100644 --- a/src/ao.h +++ b/src/ao.h @@ -21,6 +21,7 @@ #include #include #include +#include #include "cc1111.h" #define TRUE 1 @@ -423,14 +424,6 @@ ao_ee_init(void); * ao_log.c */ -/* Structure containing GPS position, either lat or lon */ - -struct ao_gps_pos { - uint8_t degrees; - uint8_t minutes; - uint16_t minutes_fraction; /* in units of 1/10000 minutes */ -}; - /* * The data log is recorded in the eeprom as a sequence * of data packets. @@ -499,8 +492,8 @@ struct ao_log_record { uint8_t second; uint8_t flags; } gps_time; - struct ao_gps_pos gps_latitude; - struct ao_gps_pos gps_longitude; + int32_t gps_latitude; + int32_t gps_longitude; struct { int16_t altitude; uint16_t unused; @@ -679,21 +672,14 @@ ao_serial_init(void); #define AO_GPS_NUM_SAT_SHIFT (0) #define AO_GPS_VALID (1 << 4) -#define AO_GPS_LONGITUDE_MASK (1 << 5) -#define AO_GPS_LONGITUDE_EAST (0 << 5) -#define AO_GPS_LONGITUDE_WEST (1 << 5) - -#define AO_GPS_LATITUDE_MASK (1 << 6) -#define AO_GPS_LATITUDE_NORTH (0 << 6) -#define AO_GPS_LATITUDE_SOUTH (1 << 6) struct ao_gps_data { uint8_t hour; uint8_t minute; uint8_t second; uint8_t flags; - struct ao_gps_pos latitude; - struct ao_gps_pos longitude; + int32_t latitude; + int32_t longitude; int16_t altitude; }; diff --git a/src/ao_gps.c b/src/ao_gps.c index cbde8b48..147b665c 100644 --- a/src/ao_gps.c +++ b/src/ao_gps.c @@ -17,237 +17,282 @@ #include "ao.h" -#define AO_GPS_LEADER 6 - -static const char ao_gps_header[] = "GPGGA,"; __xdata uint8_t ao_gps_mutex; -static __xdata char ao_gps_char; -static __xdata uint8_t ao_gps_cksum; -static __xdata uint8_t ao_gps_error; - __xdata struct ao_gps_data ao_gps_data; -static __xdata struct ao_gps_data ao_gps_next; -const char ao_gps_config[] = - "$PSRF103,00,00,01,01*25\r\n" /* GGA 1 per sec */ - "$PSRF103,01,00,00,01*25\r\n" /* GLL disable */ - "$PSRF103,02,00,00,01*26\r\n" /* GSA disable */ - "$PSRF103,03,00,00,01*27\r\n" /* GSV disable */ - "$PSRF103,04,00,00,01*20\r\n" /* RMC disable */ - "$PSRF103,05,00,00,01*21\r\n"; /* VTG disable */ +const char ao_gps_config[] = { + /* Serial param - binary, 4800 baud, 8 bits, 1 stop, no parity */ + '$', 'P', 'S', 'R', 'F', '1', '0', '0', ',', '0', ',', + '4', '8', '0', '0', ',', '8', ',', '1', ',', '0', '*', + '0', 'F', '\r','\n', + + 0xa0, 0xa2, 0x00, 0x0e, /* length: 14 bytes */ + 136, /* mode control */ + 0, 0, /* reserved */ + 4, /* degraded mode (disabled) */ + 0, 0, /* reserved */ + 0, 0, /* user specified altitude */ + 2, /* alt hold mode (disabled, require 3d fixes) */ + 0, /* alt hold source (use last computed altitude) */ + 0, /* reserved */ + 0, /* Degraded time out (disabled) */ + 0, /* Dead Reckoning time out (disabled) */ + 0, /* Track smoothing (disabled) */ + 0x00, 0x8e, 0xb0, 0xb3, + + 0xa0, 0xa2, 0x00, 0x02, /* length: 2 bytes */ + 143, /* static navigation */ + 0, /* disable */ + 0x00, 0x8f, 0xb0, 0xb3, + + 0xa0, 0xa2, 0x00, 0x08, /* length: 8 bytes */ + 166, /* Set message rate */ + 2, /* enable/disable all messages */ + 0, /* message id (ignored) */ + 0, /* update rate (0 = disable) */ + 0, 0, 0, 0, /* reserved */ + 0x00, 0xa8, 0xb0, 0xb3, + + 0xa0, 0xa2, 0x00, 0x08, /* length: 8 bytes */ + 166, /* Set message rate */ + 0, /* enable/disable one message */ + 41, /* message 41 */ + 1, /* once per second */ + 0, 0, 0, 0, /* reserved */ + 0x00, 0xd0, 0xb0, 0xb3, +}; -static void -ao_gps_lexchar(void) -{ - if (ao_gps_error) - ao_gps_char = '\n'; - else - ao_gps_char = ao_serial_getchar(); - ao_gps_cksum ^= ao_gps_char; -} +#define NAV_TYPE_GPS_FIX_TYPE_MASK (7 << 0) +#define NAV_TYPE_NO_FIX (0 << 0) +#define NAV_TYPE_SV_KF (1 << 0) +#define NAV_TYPE_2_SV_KF (2 << 0) +#define NAV_TYPE_3_SV_KF (3 << 0) +#define NAV_TYPE_4_SV_KF (4 << 0) +#define NAV_TYPE_2D_LEAST_SQUARES (5 << 0) +#define NAV_TYPE_3D_LEAST_SQUARES (6 << 0) +#define NAV_TYPE_DR (7 << 0) +#define NAV_TYPE_TRICKLE_POWER (1 << 3) +#define NAV_TYPE_ALTITUDE_HOLD_MASK (3 << 4) +#define NAV_TYPE_ALTITUDE_HOLD_NONE (0 << 4) +#define NAV_TYPE_ALTITUDE_HOLD_KF (1 << 4) +#define NAV_TYPE_ALTITUDE_HOLD_USER (2 << 4) +#define NAV_TYPE_ALTITUDE_HOLD_ALWAYS (3 << 4) +#define NAV_TYPE_DOP_LIMIT_EXCEEDED (1 << 6) +#define NAV_TYPE_DGPS_APPLIED (1 << 7) +#define NAV_TYPE_SENSOR_DR (1 << 8) +#define NAV_TYPE_OVERDETERMINED (1 << 9) +#define NAV_TYPE_DR_TIMEOUT_EXCEEDED (1 << 10) +#define NAV_TYPE_FIX_MI_EDIT (1 << 11) +#define NAV_TYPE_INVALID_VELOCITY (1 << 12) +#define NAV_TYPE_ALTITUDE_HOLD_DISABLED (1 << 13) +#define NAV_TYPE_DR_ERROR_STATUS_MASK (3 << 14) +#define NAV_TYPE_DR_ERROR_STATUS_GPS_ONLY (0 << 14) +#define NAV_TYPE_DR_ERROR_STATUS_DR_FROM_GPS (1 << 14) +#define NAV_TYPE_DR_ERROR_STATUS_DR_SENSOR_ERROR (2 << 14) +#define NAV_TYPE_DR_ERROR_STATUS_DR_IN_TEST (3 << 14) + +struct sirf_geodetic_nav_data { + uint16_t nav_type; + uint16_t utc_year; + uint8_t utc_month; + uint8_t utc_day; + uint8_t utc_hour; + uint8_t utc_minute; + uint16_t utc_second; + int32_t lat; + int32_t lon; + int32_t alt_msl; + uint16_t ground_speed; + uint16_t course; + int16_t climb_rate; + uint32_t h_error; + uint32_t v_error; + uint8_t num_sv; + uint8_t hdop; +}; -void -ao_gps_skip(void) -{ - while (ao_gps_char >= '0') - ao_gps_lexchar(); -} +static __xdata struct sirf_geodetic_nav_data ao_sirf_data; -void -ao_gps_skip_field(void) +static __pdata uint16_t ao_sirf_cksum; +static __pdata uint16_t ao_sirf_len; + +#define ao_sirf_byte() ((uint8_t) ao_serial_getchar()) + +static uint8_t data_byte(void) { - while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n') - ao_gps_lexchar(); + uint8_t c = ao_sirf_byte(); + --ao_sirf_len; + ao_sirf_cksum += c; + return c; } -void -ao_gps_skip_sep(void) +static void sirf_u16(uint8_t offset) { - while (ao_gps_char == ',' || ao_gps_char == '.' || ao_gps_char == '*') - ao_gps_lexchar(); -} + uint16_t __xdata *ptr = (uint16_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset); + uint16_t val; -__xdata static uint8_t ao_gps_num_width; + val = data_byte() << 8; + val |= data_byte (); + *ptr = val; +} -static int16_t -ao_gps_decimal(uint8_t max_width) +static void sirf_u8(uint8_t offset) { - int16_t v; - __xdata uint8_t neg = 0; + uint8_t __xdata *ptr = (uint8_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset); + uint8_t val; - ao_gps_skip_sep(); - if (ao_gps_char == '-') { - neg = 1; - ao_gps_lexchar(); - } - v = 0; - ao_gps_num_width = 0; - while (ao_gps_num_width < max_width) { - if (ao_gps_char < '0' || '9' < ao_gps_char) - break; - v = v * (int16_t) 10 + ao_gps_char - '0'; - ao_gps_num_width++; - ao_gps_lexchar(); - } - if (neg) - v = -v; - return v; + val = data_byte (); + *ptr = val; } -static uint8_t -ao_gps_hex(uint8_t max_width) +static void sirf_u32(uint8_t offset) { - uint8_t v, d; - - ao_gps_skip_sep(); - v = 0; - ao_gps_num_width = 0; - while (ao_gps_num_width < max_width) { - if ('0' <= ao_gps_char && ao_gps_char <= '9') - d = ao_gps_char - '0'; - else if ('A' <= ao_gps_char && ao_gps_char <= 'F') - d = ao_gps_char - 'A' + 10; - else if ('a' <= ao_gps_char && ao_gps_char <= 'f') - d = ao_gps_char - 'a' + 10; - else - break; - v = (v << 4) | d; - ao_gps_num_width++; - ao_gps_lexchar(); - } - return v; + uint32_t __xdata *ptr = (uint32_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset); + uint32_t val; + + val = ((uint32_t) data_byte ()) << 24; + val |= ((uint32_t) data_byte ()) << 16; + val |= ((uint32_t) data_byte ()) << 8; + val |= ((uint32_t) data_byte ()); + *ptr = val; } -static void -ao_gps_parse_pos(__xdata struct ao_gps_pos * pos, uint8_t deg_width) __reentrant +static void sirf_discard(uint8_t len) { - pos->degrees = ao_gps_decimal(deg_width); - pos->minutes = ao_gps_decimal(2); - if (ao_gps_char == '.') { - pos->minutes_fraction = ao_gps_decimal(4); - while (ao_gps_num_width < 4) { - pos->minutes_fraction *= 10; - ao_gps_num_width++; - } - } else { - pos->minutes_fraction = 0; - if (ao_gps_char != ',') - ao_gps_error = 1; - } + while (len--) + data_byte(); } +#define SIRF_END 0 +#define SIRF_DISCARD 1 +#define SIRF_U8 2 +#define SIRF_U16 3 +#define SIRF_U32 4 + +struct sirf_packet_parse { + uint8_t type; + uint8_t offset; +}; + +static const struct sirf_packet_parse geodetic_nav_data_packet[] = { + { SIRF_DISCARD, 2 }, /* 1 nav valid */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, nav_type) }, /* 3 */ + { SIRF_DISCARD, 6 }, /* 5 week number, time of week */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_year) }, /* 11 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_month) }, /* 13 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_day) }, /* 14 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_hour) }, /* 15 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_minute) }, /* 16 */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_second) }, /* 17 */ + { SIRF_DISCARD, 4 }, /* satellite id list */ /* 19 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lat) }, /* 23 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lon) }, /* 27 */ + { SIRF_DISCARD, 4 }, /* altitude from ellipsoid */ /* 31 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, alt_msl) }, /* 35 */ + { SIRF_DISCARD, 1 }, /* map datum */ /* 39 */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, ground_speed) }, /* 40 */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, course) }, /* 42 */ + { SIRF_DISCARD, 2 }, /* magnetic variation */ /* 44 */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, climb_rate) }, /* 46 */ + { SIRF_DISCARD, 2 }, /* turn rate */ /* 48 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, h_error) }, /* 50 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, v_error) }, /* 54 */ + { SIRF_DISCARD, 30 }, /* time error, h_vel error, clock_bias, + clock bias error, clock drift, + clock drift error, distance, + distance error, heading error */ /* 58 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, num_sv) }, /* 88 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, hdop) }, /* 89 */ + { SIRF_DISCARD, 1 }, /* additional mode info */ /* 90 */ + { SIRF_END, 0 }, /* 91 */ +}; + static void -ao_gps_parse_flag(char yes_c, uint8_t yes, char no_c, uint8_t no) __reentrant +ao_sirf_parse_41(void) { - ao_gps_skip_sep(); - if (ao_gps_char == yes_c) - ao_gps_next.flags |= yes; - else if (ao_gps_char == no_c) - ao_gps_next.flags |= no; - else - ao_gps_error = 1; - ao_gps_lexchar(); + uint8_t i, offset; + + for (i = 0; ; i++) { + offset = geodetic_nav_data_packet[i].offset; + switch (geodetic_nav_data_packet[i].type) { + case SIRF_END: + return; + case SIRF_DISCARD: + sirf_discard(offset); + break; + case SIRF_U8: + sirf_u8(offset); + break; + case SIRF_U16: + sirf_u16(offset); + break; + case SIRF_U32: + sirf_u32(offset); + break; + } + } } - void ao_gps(void) __reentrant { char c; uint8_t i; + uint16_t cksum; for (i = 0; (c = ao_gps_config[i]); i++) ao_serial_putchar(c); for (;;) { /* Locate the begining of the next record */ - for (;;) { - c = ao_serial_getchar(); - if (c == '$') - break; - } + while (ao_sirf_byte() != 0xa0) + ; + if (ao_sirf_byte() != 0xa2) + continue; + + /* Length */ + ao_sirf_len = ao_sirf_byte() << 8; + ao_sirf_len |= ao_sirf_byte(); + if (ao_sirf_len > 1023) + continue; + + ao_sirf_cksum = 0; - ao_gps_cksum = 0; - ao_gps_error = 0; + /* message ID */ + i = data_byte (); /* 0 */ - /* Skip anything other than GGA */ - for (i = 0; i < AO_GPS_LEADER; i++) { - ao_gps_lexchar(); - if (ao_gps_char != ao_gps_header[i]) + switch (i) { + case 41: + if (ao_sirf_len < 91) break; + ao_sirf_parse_41(); + break; } - if (i != AO_GPS_LEADER) + if (ao_sirf_len != 0) continue; - /* Now read the data into the gps data record - * - * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66 - * - * Essential fix data - * - * 025149.000 time (02:51:49.000 GMT) - * 4528.1723,N Latitude 45°28.1723' N - * 12244.2480,W Longitude 122°44.2480' W - * 1 Fix quality: - * 0 = invalid - * 1 = GPS fix (SPS) - * 2 = DGPS fix - * 3 = PPS fix - * 4 = Real Time Kinematic - * 5 = Float RTK - * 6 = estimated (dead reckoning) - * 7 = Manual input mode - * 8 = Simulation mode - * 05 Number of satellites (5) - * 2.0 Horizontal dilution - * 103.5,M Altitude, 103.5M above msl - * -19.5,M Height of geoid above WGS84 ellipsoid - * ? time in seconds since last DGPS update - * 0000 DGPS station ID - * *66 checksum - */ - - ao_gps_next.flags = 0; - ao_gps_next.hour = ao_gps_decimal(2); - ao_gps_next.minute = ao_gps_decimal(2); - ao_gps_next.second = ao_gps_decimal(2); - ao_gps_skip_field(); /* skip seconds fraction */ - - ao_gps_parse_pos(&ao_gps_next.latitude, 2); - ao_gps_parse_flag('N', AO_GPS_LATITUDE_NORTH, 'S', AO_GPS_LATITUDE_SOUTH); - ao_gps_parse_pos(&ao_gps_next.longitude, 3); - ao_gps_parse_flag('W', AO_GPS_LONGITUDE_WEST, 'E', AO_GPS_LONGITUDE_EAST); - - i = ao_gps_decimal(0xff); - if (i == 1) - ao_gps_next.flags |= AO_GPS_VALID; - - i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT; - if (i > AO_GPS_NUM_SAT_MASK) - i = AO_GPS_NUM_SAT_MASK; - ao_gps_next.flags |= i; - - ao_gps_lexchar(); - ao_gps_skip_field(); /* Horizontal dilution */ - - ao_gps_next.altitude = ao_gps_decimal(0xff); - ao_gps_skip_field(); /* skip any fractional portion */ - - /* Skip remaining fields */ - while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') { - ao_gps_lexchar(); - ao_gps_skip_field(); - } - if (ao_gps_char == '*') { - uint8_t cksum = ao_gps_cksum ^ '*'; - if (cksum != ao_gps_hex(2)) - ao_gps_error = 1; - } else - ao_gps_error = 1; - if (!ao_gps_error) { + /* verify checksum and end sequence */ + ao_sirf_cksum &= 0x7fff; + cksum = ao_sirf_byte() << 8; + cksum |= ao_sirf_byte(); + if (ao_sirf_cksum != cksum) + continue; + if (ao_sirf_byte() != 0xb0) + continue; + if (ao_sirf_byte() != 0xb3) + continue; + + switch (i) { + case 41: ao_mutex_get(&ao_gps_mutex); - memcpy(&ao_gps_data, &ao_gps_next, sizeof (struct ao_gps_data)); + ao_gps_data.hour = ao_sirf_data.utc_hour; + ao_gps_data.minute = ao_sirf_data.utc_minute; + ao_gps_data.second = ao_sirf_data.utc_second / 1000; + ao_gps_data.flags = (ao_sirf_data.num_sv << AO_GPS_NUM_SAT_SHIFT) & AO_GPS_NUM_SAT_MASK; + if ((ao_sirf_data.nav_type & NAV_TYPE_GPS_FIX_TYPE_MASK) >= NAV_TYPE_4_SV_KF) + ao_gps_data.flags |= AO_GPS_VALID; ao_mutex_put(&ao_gps_mutex); ao_wakeup(&ao_gps_data); + break; } } } diff --git a/src/ao_gps_print.c b/src/ao_gps_print.c index 4dced8f1..bef87aea 100644 --- a/src/ao_gps_print.c +++ b/src/ao_gps_print.c @@ -17,26 +17,50 @@ #include "ao.h" +struct ao_gps_split { + uint8_t positive; + uint8_t degrees; + uint8_t minutes; + uint16_t minutes_fraction; +}; + +static void +ao_gps_split(int32_t v, __xdata struct ao_gps_split *split) __reentrant +{ + uint32_t minutes_e7; + + split->positive = 1; + if (v < 0) { + v = -v; + split->positive = 0; + } + split->degrees = v / 10000000; + minutes_e7 = (v % 10000000) * 60; + split->minutes = minutes_e7 / 10000000; + split->minutes_fraction = (minutes_e7 % 10000000) / 1000; +} + void ao_gps_print(__xdata struct ao_gps_data *gps_data) __reentrant { printf("GPS %2d sat", (gps_data->flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT);; if (gps_data->flags & AO_GPS_VALID) { + static __xdata struct ao_gps_split lat, lon; + ao_gps_split(gps_data->latitude, &lat); + ao_gps_split(gps_data->longitude, &lon); printf(" %2d:%02d:%02d %2d°%02d.%04d'%c %2d°%02d.%04d'%c %5dm\n", gps_data->hour, gps_data->minute, gps_data->second, - gps_data->latitude.degrees, - gps_data->latitude.minutes, - gps_data->latitude.minutes_fraction, - (gps_data->flags & AO_GPS_LATITUDE_MASK) == AO_GPS_LATITUDE_NORTH ? - 'N' : 'S', - gps_data->longitude.degrees, - gps_data->longitude.minutes, - gps_data->longitude.minutes_fraction, - (gps_data->flags & AO_GPS_LONGITUDE_MASK) == AO_GPS_LONGITUDE_WEST ? - 'W' : 'E', + lat.degrees, + lat.minutes, + lat.minutes_fraction, + lat.positive ? 'N' : 'S', + lon.degrees, + lon.minutes, + lon.minutes_fraction, + lon.positive ? 'E' : 'W', gps_data->altitude, (gps_data->flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT); } else { diff --git a/src/ao_gps_report.c b/src/ao_gps_report.c index 1b5402a8..dce12adb 100644 --- a/src/ao_gps_report.c +++ b/src/ao_gps_report.c @@ -40,14 +40,10 @@ ao_gps_report(void) gps_log.u.gps_time.flags = gps_data.flags; ao_log_data(&gps_log); gps_log.type = AO_LOG_GPS_LAT; - gps_log.u.gps_latitude.degrees = gps_data.latitude.degrees; - gps_log.u.gps_latitude.minutes = gps_data.latitude.minutes; - gps_log.u.gps_latitude.minutes_fraction = gps_data.latitude.minutes_fraction; + gps_log.u.gps_latitude = gps_data.latitude; ao_log_data(&gps_log); gps_log.type = AO_LOG_GPS_LON; - gps_log.u.gps_longitude.degrees = gps_data.longitude.degrees; - gps_log.u.gps_longitude.minutes = gps_data.longitude.minutes; - gps_log.u.gps_longitude.minutes_fraction = gps_data.longitude.minutes_fraction; + gps_log.u.gps_longitude = gps_data.longitude; ao_log_data(&gps_log); gps_log.type = AO_LOG_GPS_ALT; gps_log.u.gps_altitude.altitude = gps_data.altitude; -- 2.30.2