X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_skytraq.c;h=050573e877282012acf1776668396f980822bfdf;hb=7f9e76e0d3492dcc8bf4b5b1f07c3c727cfdc0ef;hp=7ac269469d31c2bc90c42c6d92c0c9b5adb4b95e;hpb=1c344b760776cd5d8c0297d8db9bf02687381b4e;p=fw%2Faltos diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index 7ac26946..050573e8 100644 --- a/src/drivers/ao_gps_skytraq.c +++ b/src/drivers/ao_gps_skytraq.c @@ -19,23 +19,19 @@ #include "ao.h" #endif -#define AO_GPS_LEADER 2 - -static __code char ao_gps_header[] = "GP"; - __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 +46,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 +64,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_serial_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 +108,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 +121,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 +149,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 +165,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 +187,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; @@ -251,21 +276,12 @@ ao_nmea_gga() ao_gps_next.altitude = 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; - 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); } @@ -316,19 +332,14 @@ 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_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); } @@ -371,17 +382,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; @@ -411,11 +414,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; @@ -452,7 +456,6 @@ ao_gps(void) __reentrant if (ao_serial_getchar() == '$') { ao_gps_nmea_parse(); } - } }