X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_skytraq.c;h=3ecae435bfe52e7ed2cb4fd1a8e402fe4b7ac27f;hp=6290960677651febc0b309f3b8ea252dc92628a5;hb=6023ff81f1bbd240169b9548209133d3b02d475f;hpb=b132eefc5f63412bb4a98a4bb72b9055e40d5d42 diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index 62909606..3ecae435 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,10 +20,20 @@ #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 + +#ifndef ao_gps_putchar +#define ao_gps_putchar ao_serial1_putchar +#endif -static __code char ao_gps_header[] = "GP"; +#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 __data uint8_t ao_gps_cksum; @@ -50,7 +61,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 +83,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; } @@ -268,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); } } @@ -343,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); } } @@ -406,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); } } @@ -418,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; @@ -444,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)); @@ -456,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 -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) __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 }, };