X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_skytraq.c;h=d2f67e6b6947b1f9cd4c8e1c87fc13a018eb23b7;hb=a764bf06d0975cbf1620b079351c7437053ea1a8;hp=dd8d70ab67cf2d5acb28b1148a4d37f9e6440564;hpb=d3e8275572550b700563b3bde73cd7a43b608f17;p=fw%2Faltos diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index dd8d70ab..d2f67e6b 100644 --- a/src/drivers/ao_gps_skytraq.c +++ b/src/drivers/ao_gps_skytraq.c @@ -19,9 +19,18 @@ #include "ao.h" #endif -#define AO_GPS_LEADER 2 +#ifndef ao_gps_getchar +#define ao_gps_getchar ao_serial1_getchar +#define ao_gps_fifo ao_serial1_rx_fifo +#endif -static __code char ao_gps_header[] = "GP"; +#ifndef ao_gps_putchar +#define ao_gps_putchar ao_serial1_putchar +#endif + +#ifndef ao_gps_set_speed +#define ao_gps_set_speed ao_serial1_set_speed +#endif __xdata uint8_t ao_gps_mutex; static __data char ao_gps_char; @@ -50,7 +59,7 @@ static __pdata struct ao_telemetry_satellite ao_gps_tracking_next; (id^a^b^c^d^e^f^g^h^i^j^k^l^m^n), STQ_E static __code uint8_t ao_gps_config[] = { - SKYTRAQ_MSG_8(0x08, 1, 1, 1, 1, 1, 1, 1, 0), /* configure nmea */ + SKYTRAQ_MSG_8(0x08, 1, 0, 1, 0, 1, 0, 0, 0), /* configure nmea */ /* gga interval */ /* gsa interval */ /* gsv interval */ @@ -72,7 +81,7 @@ ao_gps_lexchar(void) if (ao_gps_error) c = '\n'; else - c = ao_serial_getchar(); + c = ao_gps_getchar(); ao_gps_cksum ^= c; ao_gps_char = c; } @@ -191,7 +200,27 @@ ao_gps_parse_flag(char no_c, char yes_c) } static void -ao_nmea_gga() +ao_nmea_finish(void) +{ + char c; + /* Skip remaining fields */ + for (;;) { + c = ao_gps_char; + if (c == '*' || c == '\n' || c == '\r') + break; + ao_gps_lexchar(); + ao_gps_skip_field(); + } + if (c == '*') { + uint8_t cksum = ao_gps_cksum ^ '*'; + if (cksum != ao_gps_hex()) + ao_gps_error = 1; + } else + ao_gps_error = 1; +} + +static void +ao_nmea_gga(void) { uint8_t i; @@ -260,21 +289,12 @@ ao_nmea_gga() 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()) - ao_gps_error = 1; - } else - ao_gps_error = 1; + ao_nmea_finish(); + if (!ao_gps_error) { ao_mutex_get(&ao_gps_mutex); ao_gps_tick = ao_gps_next_tick; - ao_xmemcpy(&ao_gps_data, &ao_gps_next, sizeof (ao_gps_data)); + ao_xmemcpy(&ao_gps_data, PDATA_TO_XDATA(&ao_gps_next), sizeof (ao_gps_data)); ao_mutex_put(&ao_gps_mutex); ao_wakeup(&ao_gps_data); } @@ -325,19 +345,14 @@ ao_nmea_gsv(void) ao_gps_tracking_next.channels = i + 1; } } - if (ao_gps_char == '*') { - uint8_t cksum = ao_gps_cksum ^ '*'; - if (cksum != ao_gps_hex()) - ao_gps_error = 1; - } - else - ao_gps_error = 1; + + ao_nmea_finish(); + if (ao_gps_error) ao_gps_tracking_next.channels = 0; else if (done) { ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&ao_gps_tracking_data, &ao_gps_tracking_next, - sizeof(ao_gps_tracking_data)); + ao_xmemcpy(&ao_gps_tracking_data, PDATA_TO_XDATA(&ao_gps_tracking_next), sizeof(ao_gps_tracking_data)); ao_mutex_put(&ao_gps_mutex); ao_wakeup(&ao_gps_tracking_data); } @@ -380,17 +395,9 @@ ao_nmea_rmc(void) a = ao_gps_decimal(2); c = ao_gps_decimal(2); i = ao_gps_decimal(2); - /* 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()) - ao_gps_error = 1; - } else - ao_gps_error = 1; + + ao_nmea_finish(); + if (!ao_gps_error) { ao_gps_next.year = i; ao_gps_next.month = c; @@ -408,7 +415,7 @@ ao_skytraq_sendbytes(__code uint8_t *b, uint8_t l) uint8_t c = *b++; if (c == 0xa0) ao_delay(AO_MS_TO_TICKS(500)); - ao_serial_putchar(c); + ao_gps_putchar(c); } } @@ -420,11 +427,12 @@ ao_gps_nmea_parse(void) ao_gps_cksum = 0; ao_gps_error = 0; - for (a = 0; a < AO_GPS_LEADER; a++) { - ao_gps_lexchar(); - if (ao_gps_char != ao_gps_header[a]) - return; - } + ao_gps_lexchar(); + if (ao_gps_char != 'G') + return; + ao_gps_lexchar(); + if (ao_gps_char != 'P') + return; ao_gps_lexchar(); a = ao_gps_char; @@ -446,10 +454,12 @@ ao_gps_nmea_parse(void) } } +static uint8_t ao_gps_updating; + void ao_gps(void) __reentrant { - ao_serial_set_speed(AO_SERIAL_SPEED_9600); + ao_gps_set_speed(AO_SERIAL_SPEED_9600); /* give skytraq time to boot in case of cold start */ ao_delay(AO_MS_TO_TICKS(2000)); @@ -458,10 +468,16 @@ ao_gps(void) __reentrant for (;;) { /* Locate the begining of the next record */ - if (ao_serial_getchar() == '$') { + if (ao_gps_getchar() == '$') { ao_gps_nmea_parse(); } - +#ifndef AO_GPS_TEST + while (ao_gps_updating) { + ao_usb_putchar(ao_gps_getchar()); + if (ao_fifo_empty(ao_gps_fifo)) + flush(); + } +#endif } } @@ -474,7 +490,7 @@ gps_dump(void) __reentrant ao_mutex_get(&ao_gps_mutex); printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day); printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second); - printf ("Lat/Lon: %ld %ld\n", ao_gps_data.latitude, ao_gps_data.longitude); + printf ("Lat/Lon: %ld %ld\n", (long) ao_gps_data.latitude, (long) ao_gps_data.longitude); printf ("Alt: %d\n", ao_gps_data.altitude); printf ("Flags: 0x%x\n", ao_gps_data.flags); printf ("Sats: %d", ao_gps_tracking_data.channels); @@ -486,8 +502,38 @@ gps_dump(void) __reentrant ao_mutex_put(&ao_gps_mutex); } +static __code uint8_t ao_gps_115200[] = { + SKYTRAQ_MSG_3(5,0,5,0) /* Set to 115200 baud */ +}; + +static void +ao_gps_set_speed_delay(uint8_t speed) { + ao_delay(AO_MS_TO_TICKS(500)); + ao_gps_set_speed(speed); + ao_delay(AO_MS_TO_TICKS(500)); +} + +static void +gps_update(void) __reentrant +{ + ao_gps_updating = 1; + ao_task_minimize_latency = 1; +#if HAS_ADC + ao_timer_set_adc_interval(0); +#endif + ao_skytraq_sendstruct(ao_gps_115200); + ao_gps_set_speed_delay(AO_SERIAL_SPEED_4800); + ao_skytraq_sendstruct(ao_gps_115200); + ao_gps_set_speed_delay(AO_SERIAL_SPEED_115200); + + /* It's a binary protocol; abandon attempts to escape */ + for (;;) + ao_gps_putchar(ao_usb_getchar()); +} + __code struct ao_cmds ao_gps_cmds[] = { { gps_dump, "g\0Display GPS" }, + { gps_update, "U\0Update GPS firmware" }, { 0, NULL }, };