X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_skytraq.c;h=193f20dc4eb4a6af61651534d1816857a7e8355c;hp=d2f67e6b6947b1f9cd4c8e1c87fc13a018eb23b7;hb=0686a7b8aec524d81bda4c572549a3a068ce0eed;hpb=816c6b5d087694a9db9c34cc5ec7671a1487d9b9 diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index d2f67e6b..193f20dc 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 @@ -32,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 @@ -58,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 */ @@ -105,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) @@ -160,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); @@ -277,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); } } @@ -352,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); } } @@ -409,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++; @@ -457,7 +462,7 @@ 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); @@ -481,28 +486,9 @@ ao_gps(void) __reentrant } } -__xdata struct ao_task ao_gps_task; - -static void -gps_dump(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", (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); -} +struct ao_task ao_gps_task; -static __code uint8_t ao_gps_115200[] = { +static const uint8_t ao_gps_115200[] = { SKYTRAQ_MSG_3(5,0,5,0) /* Set to 115200 baud */ }; @@ -514,7 +500,7 @@ ao_gps_set_speed_delay(uint8_t speed) { } static void -gps_update(void) __reentrant +gps_update(void) { ao_gps_updating = 1; ao_task_minimize_latency = 1; @@ -531,8 +517,8 @@ gps_update(void) __reentrant 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 }, };