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;
}
} 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();
}
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;
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())
- 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_tracking_next.channels = i + 1;
}
}
- if (ao_gps_char == '*') {
- uint8_t cksum = ao_gps_cksum ^ '*';
- if (cksum != ao_gps_hex())
- 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) {
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())
- 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;