X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_skytraq.c;h=c483382e03539405814b6178d04bfa29848cf655;hp=7ac269469d31c2bc90c42c6d92c0c9b5adb4b95e;hb=1085ec5d57e0ed5d132f2bbdac1a0b6a32c0ab4a;hpb=f6f54d70b768dca1715ddddea64a4df00d82b09e diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index 7ac26946..c483382e 100644 --- a/src/drivers/ao_gps_skytraq.c +++ b/src/drivers/ao_gps_skytraq.c @@ -3,7 +3,8 @@ * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of @@ -19,23 +20,33 @@ #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_new; __xdata uint8_t ao_gps_mutex; static __data char ao_gps_char; -static __pdata uint8_t ao_gps_cksum; -static __pdata uint8_t ao_gps_error; +static __data uint8_t ao_gps_cksum; +static __data uint8_t ao_gps_error; __pdata uint16_t ao_gps_tick; __xdata struct ao_telemetry_location ao_gps_data; __xdata struct ao_telemetry_satellite ao_gps_tracking_data; static __pdata uint16_t ao_gps_next_tick; -static __xdata struct ao_telemetry_location ao_gps_next; +static __pdata struct ao_telemetry_location ao_gps_next; static __pdata uint8_t ao_gps_date_flags; -static __xdata struct ao_telemetry_satellite ao_gps_tracking_next; +static __pdata struct ao_telemetry_satellite ao_gps_tracking_next; #define STQ_S 0xa0, 0xa1 #define STQ_E 0x0d, 0x0a @@ -50,7 +61,7 @@ static __xdata 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 */ @@ -68,34 +79,41 @@ static __code uint8_t ao_gps_config[] = { static void ao_gps_lexchar(void) { + char c; if (ao_gps_error) - ao_gps_char = '\n'; + c = '\n'; else - ao_gps_char = ao_serial_getchar(); - ao_gps_cksum ^= ao_gps_char; + c = ao_gps_getchar(); + ao_gps_cksum ^= c; + ao_gps_char = c; } void ao_gps_skip_field(void) { - while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n') + for (;;) { + char c = ao_gps_char; + if (c == ',' || c == '*' || c == '\n') + break; ao_gps_lexchar(); + } } void ao_gps_skip_sep(void) { - if (ao_gps_char == ',' || ao_gps_char == '.' || ao_gps_char == '*') + char c = ao_gps_char; + if (c == ',' || c == '.' || c == '*') ao_gps_lexchar(); } -__pdata static uint8_t ao_gps_num_width; +__data static uint8_t ao_gps_num_width; static int16_t ao_gps_decimal(uint8_t max_width) { int16_t v; - __pdata uint8_t neg = 0; + uint8_t neg = 0; ao_gps_skip_sep(); if (ao_gps_char == '-') { @@ -105,9 +123,10 @@ ao_gps_decimal(uint8_t max_width) v = 0; ao_gps_num_width = 0; while (ao_gps_num_width < max_width) { - if (ao_gps_char < '0' || '9' < ao_gps_char) + uint8_t c = ao_gps_char; + if (c < (uint8_t) '0' || (uint8_t) '9' < c) break; - v = v * (int16_t) 10 + ao_gps_char - '0'; + v = v * 10 + (uint8_t) (c - (uint8_t) '0'); ao_gps_num_width++; ao_gps_lexchar(); } @@ -117,23 +136,25 @@ ao_gps_decimal(uint8_t max_width) } static uint8_t -ao_gps_hex(uint8_t max_width) +ao_gps_hex(void) { - uint8_t v, d; + uint8_t v; 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; + while (ao_gps_num_width < 2) { + uint8_t c = ao_gps_char; + uint8_t d; + if ((uint8_t) '0' <= c && c <= (uint8_t) '9') + d = - '0'; + else if ((uint8_t) 'A' <= c && c <= (uint8_t) 'F') + d = - 'A' + 10; + else if ((uint8_t) 'a' <= c && c <= (uint8_t) 'f') + d = - 'a' + 10; else break; - v = (v << 4) | d; + v = (v << 4) | (c + d); ao_gps_num_width++; ao_gps_lexchar(); } @@ -143,13 +164,15 @@ ao_gps_hex(uint8_t max_width) static int32_t ao_gps_parse_pos(uint8_t deg_width) __reentrant { - int32_t d; - int32_t m; - int32_t f; + static __pdata uint16_t d; + static __pdata uint8_t m; + static __pdata uint16_t f; + char c; d = ao_gps_decimal(deg_width); m = ao_gps_decimal(2); - if (ao_gps_char == '.') { + c = ao_gps_char; + if (c == '.') { f = ao_gps_decimal(4); while (ao_gps_num_width < 4) { f *= 10; @@ -157,17 +180,14 @@ ao_gps_parse_pos(uint8_t deg_width) __reentrant } } else { f = 0; - if (ao_gps_char != ',') + if (c != ',') ao_gps_error = 1; } - d = d * 10000000l; - m = m * 10000l + f; - d = d + m * 50 / 3; - return d; + return d * 10000000l + (m * 10000l + f) * 50 / 3; } static uint8_t -ao_gps_parse_flag(char no_c, char yes_c) __reentrant +ao_gps_parse_flag(char no_c, char yes_c) { uint8_t ret = 0; ao_gps_skip_sep(); @@ -182,7 +202,27 @@ ao_gps_parse_flag(char no_c, char yes_c) __reentrant } 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; @@ -239,35 +279,28 @@ ao_nmea_gga() ao_gps_lexchar(); i = ao_gps_decimal(0xff); - if (i <= 50) { - i = (uint8_t) 5 * i; + if (i <= 25) { + i = (uint8_t) 10 * i; if (ao_gps_char == '.') - i = (i + ((uint8_t) ao_gps_decimal(1) >> 1)); + i = (i + ((uint8_t) ao_gps_decimal(1))); } else i = 255; ao_gps_next.hdop = i; ao_gps_skip_field(); - ao_gps_next.altitude = ao_gps_decimal(0xff); + AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_next, 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; + ao_nmea_finish(); + if (!ao_gps_error) { ao_mutex_get(&ao_gps_mutex); + ao_gps_new |= AO_GPS_NEW_DATA; ao_gps_tick = ao_gps_next_tick; - memcpy(&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); + ao_wakeup(&ao_gps_new); } } @@ -316,21 +349,17 @@ 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(2)) - 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); - memcpy(&ao_gps_tracking_data, &ao_gps_tracking_next, - sizeof(ao_gps_tracking_data)); + ao_gps_new |= AO_GPS_NEW_TRACKING; + 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); + ao_wakeup(&ao_gps_new); } } @@ -371,17 +400,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(2)) - 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; @@ -399,7 +420,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); } } @@ -411,11 +432,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; @@ -437,10 +459,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)); @@ -449,36 +473,53 @@ 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 } } __xdata struct ao_task ao_gps_task; +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_dump(void) __reentrant +gps_update(void) __reentrant { - uint8_t i; - 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 ("Alt: %d\n", ao_gps_data.altitude); - printf ("Flags: 0x%x\n", ao_gps_data.flags); - printf ("Sats: %d", ao_gps_tracking_data.channels); - for (i = 0; i < ao_gps_tracking_data.channels; i++) - printf (" %d %d", - ao_gps_tracking_data.sats[i].svid, - ao_gps_tracking_data.sats[i].c_n_1); - printf ("\ndone\n"); - ao_mutex_put(&ao_gps_mutex); + 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" }, + { ao_gps_show, "g\0Display GPS" }, + { gps_update, "U\0Update GPS firmware" }, { 0, NULL }, };