X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_skytraq.c;h=f767118ab10883fd6f52d1382601810df31ff5e6;hp=d80da97ca6ea44a2cd2828380afa55eda19f0a0b;hb=6aeb000899ad7f22e92b487b5891664554bc4d33;hpb=cf44ea354c2d1780cee343132c6058e11e9eefa3 diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index d80da97c..f767118a 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 @@ -21,6 +22,7 @@ #ifndef ao_gps_getchar #define ao_gps_getchar ao_serial1_getchar +#define ao_gps_fifo ao_serial1_rx_fifo #endif #ifndef ao_gps_putchar @@ -31,19 +33,20 @@ #define ao_gps_set_speed ao_serial1_set_speed #endif -__xdata uint8_t ao_gps_mutex; -static __data char ao_gps_char; -static __data uint8_t ao_gps_cksum; -static __data uint8_t ao_gps_error; +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; -__pdata uint16_t ao_gps_tick; -__xdata struct ao_telemetry_location ao_gps_data; -__xdata struct ao_telemetry_satellite ao_gps_tracking_data; +uint16_t ao_gps_tick; +struct ao_telemetry_location ao_gps_data; +struct ao_telemetry_satellite ao_gps_tracking_data; -static __pdata uint16_t ao_gps_next_tick; -static __pdata struct ao_telemetry_location ao_gps_next; -static __pdata uint8_t ao_gps_date_flags; -static __pdata struct ao_telemetry_satellite ao_gps_tracking_next; +static uint16_t 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 @@ -57,7 +60,7 @@ static __pdata 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[] = { +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 */ @@ -85,7 +88,7 @@ ao_gps_lexchar(void) ao_gps_char = c; } -void +static void ao_gps_skip_field(void) { for (;;) { @@ -96,7 +99,7 @@ ao_gps_skip_field(void) } } -void +static void ao_gps_skip_sep(void) { char c = ao_gps_char; @@ -104,7 +107,7 @@ ao_gps_skip_sep(void) ao_gps_lexchar(); } -__data static uint8_t ao_gps_num_width; +static uint8_t ao_gps_num_width; static int16_t ao_gps_decimal(uint8_t max_width) @@ -159,11 +162,11 @@ ao_gps_hex(void) } static int32_t -ao_gps_parse_pos(uint8_t deg_width) __reentrant +ao_gps_parse_pos(uint8_t deg_width) { - static __pdata uint16_t d; - static __pdata uint8_t m; - static __pdata uint16_t f; + static uint16_t d; + static uint8_t m; + static uint16_t f; char c; d = ao_gps_decimal(deg_width); @@ -276,26 +279,28 @@ ao_nmea_gga(void) 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 */ 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; - ao_xmemcpy(&ao_gps_data, PDATA_TO_XDATA(&ao_gps_next), sizeof (ao_gps_data)); + ao_xmemcpy(&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); } } @@ -351,9 +356,10 @@ ao_nmea_gsv(void) ao_gps_tracking_next.channels = 0; else if (done) { ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&ao_gps_tracking_data, PDATA_TO_XDATA(&ao_gps_tracking_next), sizeof(ao_gps_tracking_data)); + ao_gps_new |= AO_GPS_NEW_TRACKING; + ao_xmemcpy(&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); } } @@ -408,7 +414,7 @@ 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++; @@ -453,8 +459,10 @@ ao_gps_nmea_parse(void) } } +static uint8_t ao_gps_updating; + void -ao_gps(void) __reentrant +ao_gps(void) { ao_gps_set_speed(AO_SERIAL_SPEED_9600); @@ -468,32 +476,50 @@ ao_gps(void) __reentrant 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 +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) { - 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", (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); - 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 }, };