From: Keith Packard Date: Sun, 19 Apr 2009 06:17:45 +0000 (-0700) Subject: Parse GPS data locally. Add 'g' command to display recent GPS results. X-Git-Tag: sn1-flight1~2 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=c4e983daa4579896b227fdcb2be43fad75e94307;ds=sidebyside Parse GPS data locally. Add 'g' command to display recent GPS results. This parses the GPS GGA message and stores it in a global variable, protected by a mutex. --- diff --git a/ao.h b/ao.h index 20c2458c..bbf358e9 100644 --- a/ao.h +++ b/ao.h @@ -477,15 +477,15 @@ enum ao_flight_state { ao_flight_invalid }; -extern __xdata struct ao_adc ao_flight_data; -extern __data enum flight_state ao_flight_state; -extern __data uint16_t ao_flight_tick; -extern __data int16_t ao_flight_accel; -extern __data int16_t ao_flight_pres; -extern __data int16_t ao_ground_pres; -extern __data int16_t ao_ground_accel; -extern __data int16_t ao_min_pres; -extern __data uint16_t ao_launch_time; +extern __xdata struct ao_adc ao_flight_data; +extern __pdata enum flight_state ao_flight_state; +extern __pdata uint16_t ao_flight_tick; +extern __pdata int16_t ao_flight_accel; +extern __pdata int16_t ao_flight_pres; +extern __pdata int16_t ao_ground_pres; +extern __pdata int16_t ao_ground_accel; +extern __pdata int16_t ao_min_pres; +extern __pdata uint16_t ao_launch_time; /* Flight thread */ void @@ -585,6 +585,37 @@ ao_serial_init(void); * ao_gps.c */ +struct ao_gps_pos { + uint8_t degrees; + uint8_t minutes; + uint16_t minutes_fraction; /* in units of 1/10000 minutes */ +}; + +#define AO_GPS_NUM_SAT_MASK (0xf << 0) +#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; + int16_t altitude; +}; + +extern __xdata uint8_t ao_gps_mutex; +extern __xdata struct ao_gps_data ao_gps_data; + void ao_gps(void); diff --git a/ao_cmd.c b/ao_cmd.c index 575ed69a..6e91a72d 100644 --- a/ao_cmd.c +++ b/ao_cmd.c @@ -252,6 +252,34 @@ adc_dump(void) put_string("\n"); } +static void +gps_dump(void) __reentrant +{ + ao_mutex_get(&ao_gps_mutex); + if (ao_gps_data.flags & AO_GPS_VALID) { + printf("GPS %2d:%02d:%02d %2d°%2d.%04d'%c %2d°%2d.%04d'%c %5dm %2d sat\n", + ao_gps_data.hour, + ao_gps_data.minute, + ao_gps_data.second, + ao_gps_data.latitude.degrees, + ao_gps_data.latitude.minutes, + ao_gps_data.latitude.minutes_fraction, + (ao_gps_data.flags & AO_GPS_LATITUDE_MASK) == AO_GPS_LATITUDE_NORTH ? + 'N' : 'S', + ao_gps_data.longitude.degrees, + ao_gps_data.longitude.minutes, + ao_gps_data.longitude.minutes_fraction, + (ao_gps_data.flags & AO_GPS_LONGITUDE_MASK) == AO_GPS_LONGITUDE_WEST ? + 'W' : 'E', + ao_gps_data.altitude, + (ao_gps_data.flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT); + } else { + printf("GPS %2d sat\n", + (ao_gps_data.flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT);; + } + ao_mutex_put(&ao_gps_mutex); +} + static void dump(void) { @@ -488,6 +516,7 @@ static const uint8_t help_txt[] = "All numbers are in hex\n" "? Print this message\n" "a Display current ADC values\n" + "g Display current GPS values\n" "d Dump memory\n" "e Dump a block of EEPROM data\n" "w ... Write data to EEPROM\n" @@ -546,6 +575,9 @@ ao_cmd(void *parameters) case 'a': adc_dump(); break; + case 'g': + gps_dump(); + break; case 'e': ee_dump(); break; diff --git a/ao_gps.c b/ao_gps.c index e689b704..83f44d55 100644 --- a/ao_gps.c +++ b/ao_gps.c @@ -19,13 +19,16 @@ __xdata struct ao_task ao_gps_task; -#define AO_GPS_MAX_LINE 80 - #define AO_GPS_LEADER 6 -__xdata uint8_t ao_gps_line[AO_GPS_MAX_LINE+1] = "GPGGA,"; +static const uint8_t ao_gps_header[] = "GPGGA,"; __xdata uint8_t ao_gps_mutex; -__xdata uint8_t ao_gps_data; +static __xdata uint8_t 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 uint8_t ao_gps_config[] = "$PSRF103,00,00,01,01*25\r\n" /* GGA 1 per sec */ @@ -35,9 +38,122 @@ const uint8_t ao_gps_config[] = "$PSRF103,04,00,00,01*20\r\n" /* RMC disable */ "$PSRF103,05,00,00,01*21\r\n"; /* VTG disable */ +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; +} + +void +ao_gps_skip(void) +{ + while (ao_gps_char >= '0') + ao_gps_lexchar(); +} + +void +ao_gps_skip_field(void) +{ + while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n') + ao_gps_lexchar(); +} + +void +ao_gps_skip_sep(void) +{ + while (ao_gps_char == ',' || ao_gps_char == '.' || ao_gps_char == '*') + ao_gps_lexchar(); +} + +__xdata static uint8_t ao_gps_num_width; + +static int16_t +ao_gps_decimal(uint8_t max_width) +{ + int16_t v; + __xdata uint8_t neg = 0; + + 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; +} + +static uint8_t +ao_gps_hex(uint8_t max_width) +{ + 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; +} + +static void +ao_gps_parse_pos(__xdata struct ao_gps_pos * pos, uint8_t deg_width) __reentrant +{ + 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; + } +} + +static void +ao_gps_parse_flag(uint8_t yes_c, uint8_t yes, uint8_t no_c, uint8_t no) __reentrant +{ + 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(); +} + void -ao_gps(void) +ao_gps(void) __reentrant { uint8_t c; uint8_t i; @@ -45,35 +161,102 @@ ao_gps(void) 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; } - for (i = 0; i < AO_GPS_LEADER; i++) - if (ao_serial_getchar() != ao_gps_line[i]) + + ao_gps_cksum = 0; + ao_gps_error = 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]) break; + } if (i != AO_GPS_LEADER) continue; - ao_mutex_get(&ao_gps_mutex); - for (;;) { - c = ao_serial_getchar(); - if (c < ' ') - break; - if (i < AO_GPS_MAX_LINE) - ao_gps_line[i++] = c; + + /* 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 == '*') { + c = ao_gps_cksum ^ '*'; + i = ao_gps_hex(2); + if (c != i) + ao_gps_error = 1; + } else + ao_gps_error = 1; + if (!ao_gps_error) { + ao_mutex_get(&ao_gps_mutex); + memcpy(&ao_gps_data, &ao_gps_next, sizeof (ao_gps_data)); + ao_mutex_put(&ao_gps_mutex); + ao_wakeup(&ao_gps_data); } - ao_gps_line[i] = '\0'; - ao_gps_data = 1; - ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_data); - puts(ao_gps_line); - ao_usb_flush(); } } void ao_gps_init(void) { - ao_add_task(&ao_gps_task, ao_gps); + ao_add_task(&ao_gps_task, ao_gps, "gps"); }