X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_gps_skytraq.c;h=7ac269469d31c2bc90c42c6d92c0c9b5adb4b95e;hp=a2d5f1dbe9cd9e988e5851fada1211a815ace17b;hb=e268798dc260311f5f0167909481b41c9d27fc1c;hpb=8a775b8f9ecefa143050653d74dfd218b32b9bb5 diff --git a/src/ao_gps_skytraq.c b/src/ao_gps_skytraq.c index a2d5f1db..7ac26946 100644 --- a/src/ao_gps_skytraq.c +++ b/src/ao_gps_skytraq.c @@ -19,42 +19,50 @@ #include "ao.h" #endif -#define AO_GPS_LEADER 2 +#define AO_GPS_LEADER 2 -static const char ao_gps_header[] = "GP"; +static __code char ao_gps_header[] = "GP"; __xdata uint8_t ao_gps_mutex; -static __xdata char ao_gps_char; -static __xdata uint8_t ao_gps_cksum; -static __xdata uint8_t ao_gps_error; - -__xdata uint16_t ao_gps_tick; -__xdata struct ao_gps_data ao_gps_data; -__xdata struct ao_gps_tracking_data ao_gps_tracking_data; - -static __xdata uint16_t ao_gps_next_tick; -static __xdata struct ao_gps_data ao_gps_next; -static __xdata uint8_t ao_gps_date_flags; -static __xdata struct ao_gps_tracking_data ao_gps_tracking_next; - -static const char ao_gps_config[] = { - 0xa0, 0xa1, 0x00, 0x09, /* length 9 bytes */ - 0x08, /* configure nmea */ - 1, /* gga interval */ - 1, /* gsa interval */ - 1, /* gsv interval */ - 1, /* gll interval */ - 1, /* rmc interval */ - 1, /* vtg interval */ - 1, /* zda interval */ - 0, /* attributes (0 = update to sram, 1 = update flash too) */ - 0x09, 0x0d, 0x0a, - - 0xa0, 0xa1, 0x00, 0x03, /* length: 3 bytes */ - 0x3c, /* configure navigation mode */ - 0x00, /* 0 = car, 1 = pedestrian */ - 0x00, /* 0 = update to sram, 1 = update sram + flash */ - 0x3c, 0x0d, 0x0a, +static __data char ao_gps_char; +static __pdata uint8_t ao_gps_cksum; +static __pdata 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 uint8_t ao_gps_date_flags; +static __xdata struct ao_telemetry_satellite ao_gps_tracking_next; + +#define STQ_S 0xa0, 0xa1 +#define STQ_E 0x0d, 0x0a +#define SKYTRAQ_MSG_2(id,a,b) \ + STQ_S, 0, 3, id, a,b, (id^a^b), STQ_E +#define SKYTRAQ_MSG_3(id,a,b,c) \ + STQ_S, 0, 4, id, a,b,c, (id^a^b^c), STQ_E +#define SKYTRAQ_MSG_8(id,a,b,c,d,e,f,g,h) \ + STQ_S, 0, 9, id, a,b,c,d,e,f,g,h, (id^a^b^c^d^e^f^g^h), STQ_E +#define SKYTRAQ_MSG_14(id,a,b,c,d,e,f,g,h,i,j,k,l,m,n) \ + 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[] = { + SKYTRAQ_MSG_8(0x08, 1, 1, 1, 1, 1, 1, 1, 0), /* configure nmea */ + /* gga interval */ + /* gsa interval */ + /* gsv interval */ + /* gll interval */ + /* rmc interval */ + /* vtg interval */ + /* zda interval */ + /* attributes (0 = update to sram, 1 = update flash too) */ + + SKYTRAQ_MSG_2(0x3c, 0x00, 0x00), /* configure navigation mode */ + /* 0 = car, 1 = pedestrian */ + /* 0 = update to sram, 1 = update sram + flash */ }; static void @@ -67,13 +75,6 @@ ao_gps_lexchar(void) ao_gps_cksum ^= ao_gps_char; } -void -ao_gps_skip(void) -{ - while (ao_gps_char >= '0') - ao_gps_lexchar(); -} - void ao_gps_skip_field(void) { @@ -88,13 +89,13 @@ ao_gps_skip_sep(void) ao_gps_lexchar(); } -__xdata static uint8_t ao_gps_num_width; +__pdata static uint8_t ao_gps_num_width; static int16_t ao_gps_decimal(uint8_t max_width) { int16_t v; - __xdata uint8_t neg = 0; + __pdata uint8_t neg = 0; ao_gps_skip_sep(); if (ao_gps_char == '-') { @@ -237,14 +238,14 @@ ao_nmea_gga() ao_gps_next.flags |= i; ao_gps_lexchar(); - ao_gps_next.hdop = ao_gps_decimal(0xff); - if (ao_gps_next.hdop <= 50) { - ao_gps_next.hdop = (uint8_t) 5 * ao_gps_next.hdop; + i = ao_gps_decimal(0xff); + if (i <= 50) { + i = (uint8_t) 5 * i; if (ao_gps_char == '.') - ao_gps_next.hdop = (ao_gps_next.hdop + - ((uint8_t) ao_gps_decimal(1) >> 1)); + i = (i + ((uint8_t) ao_gps_decimal(1) >> 1)); } else - ao_gps_next.hdop = 255; + i = 255; + ao_gps_next.hdop = i; ao_gps_skip_field(); ao_gps_next.altitude = ao_gps_decimal(0xff); @@ -264,7 +265,7 @@ ao_nmea_gga() 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 (struct ao_gps_data)); + memcpy(&ao_gps_data, &ao_gps_next, sizeof (ao_gps_data)); ao_mutex_put(&ao_gps_mutex); ao_wakeup(&ao_gps_data); } @@ -311,9 +312,8 @@ ao_nmea_gsv(void) ao_gps_skip_field(); /* azimuth */ c = ao_gps_decimal(2); /* C/N0 */ if (i < AO_MAX_GPS_TRACKING) { - if (!(ao_gps_tracking_next.sats[i].c_n_1 = c)) - ao_gps_tracking_next.sats[i].svid = 0; - ao_gps_tracking_next.channels = i + 1; + if ((ao_gps_tracking_next.sats[i].c_n_1 = c) != 0) + ao_gps_tracking_next.channels = i + 1; } } if (ao_gps_char == '*') { @@ -390,53 +390,69 @@ 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) +{ + while (l--) { + uint8_t c = *b++; + if (c == 0xa0) + ao_delay(AO_MS_TO_TICKS(500)); + ao_serial_putchar(c); + } +} + +static void +ao_gps_nmea_parse(void) +{ + uint8_t a, b, c; + + 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(); + a = ao_gps_char; + ao_gps_lexchar(); + b = ao_gps_char; + ao_gps_lexchar(); + c = ao_gps_char; + ao_gps_lexchar(); + + if (ao_gps_char != ',') + return; + + if (a == (uint8_t) 'G' && b == (uint8_t) 'G' && c == (uint8_t) 'A') { + ao_nmea_gga(); + } else if (a == (uint8_t) 'G' && b == (uint8_t) 'S' && c == (uint8_t) 'V') { + ao_nmea_gsv(); + } else if (a == (uint8_t) 'R' && b == (uint8_t) 'M' && c == (uint8_t) 'C') { + ao_nmea_rmc(); + } +} + void ao_gps(void) __reentrant { - char a, c; - uint8_t i; - ao_serial_set_speed(AO_SERIAL_SPEED_9600); - for (i = 0; i < sizeof (ao_gps_config); i++) - ao_serial_putchar(ao_gps_config[i]); - for (;;) { - /* Locate the begining of the next record */ - for (;;) { - c = ao_serial_getchar(); - if (c == '$') - break; - } - ao_gps_cksum = 0; - ao_gps_error = 0; + /* give skytraq time to boot in case of cold start */ + ao_delay(AO_MS_TO_TICKS(2000)); - /* Skip anything other than GP */ - for (i = 0; i < AO_GPS_LEADER; i++) { - ao_gps_lexchar(); - if (ao_gps_char != ao_gps_header[i]) - break; - } - if (i != AO_GPS_LEADER) - continue; + ao_skytraq_sendstruct(ao_gps_config); - /* pull the record identifier characters off the link */ - ao_gps_lexchar(); - a = ao_gps_char; - ao_gps_lexchar(); - c = ao_gps_char; - ao_gps_lexchar(); - i = ao_gps_char; - ao_gps_lexchar(); - if (ao_gps_char != ',') - continue; - - if (a == (uint8_t) 'G' && c == (uint8_t) 'G' && i == (uint8_t) 'A') { - ao_nmea_gga(); - } else if (a == (uint8_t) 'G' && c == (uint8_t) 'S' && i == (uint8_t) 'V') { - ao_nmea_gsv(); - } else if (a == (uint8_t) 'R' && c == (uint8_t) 'M' && i == (uint8_t) 'C') { - ao_nmea_rmc(); + for (;;) { + /* Locate the begining of the next record */ + if (ao_serial_getchar() == '$') { + ao_gps_nmea_parse(); } + } } @@ -445,18 +461,25 @@ __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", 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); } __code struct ao_cmds ao_gps_cmds[] = { - { 'g', gps_dump, "g Display current GPS values" }, - { 0, gps_dump, NULL }, + { gps_dump, "g\0Display GPS" }, + { 0, NULL }, }; void