X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_skytraq.c;h=3b4a62ec1a521507da3f2b95e72fd4c8033599ae;hp=7ac269469d31c2bc90c42c6d92c0c9b5adb4b95e;hb=HEAD;hpb=1c344b760776cd5d8c0297d8db9bf02687381b4e diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index 7ac26946..00c6932f 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,34 @@ #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 -__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; +#ifndef ao_gps_set_speed +#define ao_gps_set_speed ao_serial1_set_speed +#endif -__pdata uint16_t ao_gps_tick; -__xdata struct ao_telemetry_location ao_gps_data; -__xdata struct ao_telemetry_satellite ao_gps_tracking_data; +uint8_t ao_gps_new; +uint8_t ao_gps_mutex; +static char ao_gps_char; +static uint8_t ao_gps_cksum; +static uint8_t ao_gps_error; -static __pdata uint16_t ao_gps_next_tick; -static __xdata 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; +AO_TICK_TYPE ao_gps_tick; +AO_TICK_TYPE ao_gps_utc_tick; +struct ao_telemetry_location ao_gps_data; +struct ao_telemetry_satellite ao_gps_tracking_data; + +static AO_TICK_TYPE ao_gps_next_tick; +static struct ao_telemetry_location ao_gps_next; +static uint8_t ao_gps_date_flags; +static struct ao_telemetry_satellite ao_gps_tracking_next; #define STQ_S 0xa0, 0xa1 #define STQ_E 0x0d, 0x0a @@ -49,8 +61,8 @@ static __xdata struct ao_telemetry_satellite ao_gps_tracking_next; STQ_S, 0,15, id, a,b,c,d,e,f,g,h,i,j,k,l,m,n, \ (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 */ +static const uint8_t ao_gps_config[] = { + SKYTRAQ_MSG_8(0x08, 1, 0, 1, 0, 1, 0, 0, 0), /* configure nmea */ /* gga interval */ /* gsa interval */ /* gsv interval */ @@ -68,34 +80,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 +static 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 +static 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; +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 +124,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 = (int16_t) (v * 10 + (uint8_t) (c - (uint8_t) '0')); ao_gps_num_width++; ao_gps_lexchar(); } @@ -117,23 +137,24 @@ 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; + if ((uint8_t) '0' <= c && c <= (uint8_t) '9') + c -= '0'; + else if ((uint8_t) 'A' <= c && c <= (uint8_t) 'F') + c -= 'A' - 10; + else if ((uint8_t) 'a' <= c && c <= (uint8_t) 'f') + c -= 'a' - 10; else break; - v = (v << 4) | d; + v = (uint8_t) ((v << 4) | c); ao_gps_num_width++; ao_gps_lexchar(); } @@ -141,15 +162,17 @@ ao_gps_hex(uint8_t max_width) } static int32_t -ao_gps_parse_pos(uint8_t deg_width) __reentrant +ao_gps_parse_pos(uint8_t deg_width) { - int32_t d; - int32_t m; - int32_t f; + int16_t d; + int16_t m; + int16_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; @@ -216,9 +256,9 @@ ao_nmea_gga() ao_gps_next_tick = ao_time(); ao_gps_next.flags = AO_GPS_RUNNING | ao_gps_date_flags; - 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_next.hour = (uint8_t) ao_gps_decimal(2); + ao_gps_next.minute = (uint8_t) ao_gps_decimal(2); + ao_gps_next.second = (uint8_t) ao_gps_decimal(2); ao_gps_skip_field(); /* skip seconds fraction */ ao_gps_next.latitude = ao_gps_parse_pos(2); @@ -228,53 +268,46 @@ ao_nmea_gga() if (ao_gps_parse_flag('E', 'W')) ao_gps_next.longitude = -ao_gps_next.longitude; - i = ao_gps_decimal(0xff); + i = (uint8_t) ao_gps_decimal(0xff); if (i == 1) ao_gps_next.flags |= AO_GPS_VALID; - i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT; + i = (uint8_t) (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(); - i = ao_gps_decimal(0xff); - if (i <= 50) { - i = (uint8_t) 5 * i; + i = (uint8_t) ao_gps_decimal(0xff); + 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_tick = ao_gps_next_tick; + ao_gps_new |= AO_GPS_NEW_DATA; + ao_gps_tick = ao_gps_utc_tick = ao_gps_next_tick; memcpy(&ao_gps_data, &ao_gps_next, sizeof (ao_gps_data)); ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_data); + ao_wakeup(&ao_gps_new); } } static void ao_nmea_gsv(void) { - char c; + uint8_t c; uint8_t i; uint8_t done; /* Now read the data into the GPS tracking data record @@ -293,8 +326,8 @@ ao_nmea_gsv(void) * ... other SVIDs * 72 checksum */ - c = ao_gps_decimal(1); /* total messages */ - i = ao_gps_decimal(1); /* message sequence */ + c = (uint8_t) ao_gps_decimal(1); /* total messages */ + i = (uint8_t) ao_gps_decimal(1); /* message sequence */ if (i == 1) { ao_gps_tracking_next.channels = 0; } @@ -303,41 +336,37 @@ ao_nmea_gsv(void) ao_gps_skip_field(); /* sats in view */ while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') { i = ao_gps_tracking_next.channels; - c = ao_gps_decimal(2); /* SVID */ + c = (uint8_t) ao_gps_decimal(2); /* SVID */ if (i < AO_MAX_GPS_TRACKING) ao_gps_tracking_next.sats[i].svid = c; ao_gps_lexchar(); ao_gps_skip_field(); /* elevation */ ao_gps_lexchar(); ao_gps_skip_field(); /* azimuth */ - c = ao_gps_decimal(2); /* C/N0 */ + c = (uint8_t) ao_gps_decimal(2); /* C/N0 */ if (i < AO_MAX_GPS_TRACKING) { if ((ao_gps_tracking_next.sats[i].c_n_1 = c) != 0) 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; + memcpy(&ao_gps_tracking_data, &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); } } static void ao_nmea_rmc(void) { - char a, c; + uint8_t a, c; uint8_t i; /* Parse the RMC record to read out the current date */ @@ -368,20 +397,12 @@ ao_nmea_rmc(void) ao_gps_lexchar(); ao_gps_skip_field(); } - 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; + a = (uint8_t) ao_gps_decimal(2); + c = (uint8_t) ao_gps_decimal(2); + i = (uint8_t) ao_gps_decimal(2); + + ao_nmea_finish(); + if (!ao_gps_error) { ao_gps_next.year = i; ao_gps_next.month = c; @@ -393,13 +414,13 @@ ao_nmea_rmc(void) #define ao_skytraq_sendstruct(s) ao_skytraq_sendbytes((s), sizeof(s)) static void -ao_skytraq_sendbytes(__code uint8_t *b, uint8_t l) +ao_skytraq_sendbytes(const uint8_t *b, uint8_t l) { while (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_gps(void) { - 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; +struct ao_task ao_gps_task; + +static const uint8_t ao_gps_115200[] = { + SKYTRAQ_MSG_3(5,0,5,0) /* Set to 115200 baud */ +}; static void -gps_dump(void) __reentrant +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) { - 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" }, +const struct ao_cmds ao_gps_cmds[] = { + { ao_gps_show, "g\0Display GPS" }, + { gps_update, "U\0Update GPS firmware" }, { 0, NULL }, };