X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_gps_skytraq.c;h=6099ca96896bdf742e72982400f885d99d92148d;hp=c822f7fab57bf2125922f3fac5263369d74dcaf2;hb=9e5e4c1ad82d621ceb7286f72c87eeaf5976f9bf;hpb=5f2f6a8f9ba56be867888758848bc7f152ccbd47 diff --git a/src/ao_gps_skytraq.c b/src/ao_gps_skytraq.c index c822f7fa..6099ca96 100644 --- a/src/ao_gps_skytraq.c +++ b/src/ao_gps_skytraq.c @@ -19,7 +19,7 @@ #include "ao.h" #endif -#define AO_GPS_LEADER 2 +#define AO_GPS_LEADER 2 static const char ao_gps_header[] = "GP"; @@ -37,24 +37,32 @@ 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, +#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 const 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) { @@ -180,235 +181,277 @@ ao_gps_parse_flag(char no_c, char yes_c) __reentrant return ret; } - -void -ao_gps(void) __reentrant +static void +ao_nmea_gga() { - 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; - } + /* Now read the data into the gps data record + * + * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66 + * + * Essential fix data + * + * 025149.000 time (02:51:49.000 GMT) + * 4528.1723,N Latitude 45°28.1723' N + * 12244.2480,W Longitude 122°44.2480' W + * 1 Fix quality: + * 0 = invalid + * 1 = GPS fix (SPS) + * 2 = DGPS fix + * 3 = PPS fix + * 4 = Real Time Kinematic + * 5 = Float RTK + * 6 = estimated (dead reckoning) + * 7 = Manual input mode + * 8 = Simulation mode + * 05 Number of satellites (5) + * 2.0 Horizontal dilution + * 103.5,M Altitude, 103.5M above msl + * -19.5,M Height of geoid above WGS84 ellipsoid + * ? time in seconds since last DGPS update + * 0000 DGPS station ID + * *66 checksum + */ + + ao_gps_next_tick = ao_time(); + ao_gps_next.flags = AO_GPS_RUNNING | ao_gps_date_flags; + ao_gps_next.hour = ao_gps_decimal(2); + ao_gps_next.minute = ao_gps_decimal(2); + ao_gps_next.second = ao_gps_decimal(2); + ao_gps_skip_field(); /* skip seconds fraction */ + + ao_gps_next.latitude = ao_gps_parse_pos(2); + if (ao_gps_parse_flag('N', 'S')) + ao_gps_next.latitude = -ao_gps_next.latitude; + ao_gps_next.longitude = ao_gps_parse_pos(3); + if (ao_gps_parse_flag('E', 'W')) + ao_gps_next.longitude = -ao_gps_next.longitude; + + i = ao_gps_decimal(0xff); + if (i == 1) + ao_gps_next.flags |= AO_GPS_VALID; + + i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT; + if (i > AO_GPS_NUM_SAT_MASK) + i = AO_GPS_NUM_SAT_MASK; + ao_gps_next.flags |= i; - ao_gps_cksum = 0; - ao_gps_error = 0; + 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; + if (ao_gps_char == '.') + ao_gps_next.hdop = (ao_gps_next.hdop + + ((uint8_t) ao_gps_decimal(1) >> 1)); + } else + ao_gps_next.hdop = 255; + ao_gps_skip_field(); + + 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; + 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)); + ao_mutex_put(&ao_gps_mutex); + ao_wakeup(&ao_gps_data); + } +} - /* 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; +static void +ao_nmea_gsv(void) +{ + char c; + uint8_t i; + uint8_t done; + /* Now read the data into the GPS tracking data record + * + * $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72 + * + * Satellites in view data + * + * 3 Total number of GSV messages + * 1 Sequence number of current GSV message + * 12 Total sats in view (0-12) + * 05 SVID + * 54 Elevation + * 069 Azimuth + * 45 C/N0 in dB + * ... other SVIDs + * 72 checksum + */ + c = ao_gps_decimal(1); /* total messages */ + i = ao_gps_decimal(1); /* message sequence */ + if (i == 1) { + ao_gps_tracking_next.channels = 0; + } + done = (uint8_t) c == i; + ao_gps_lexchar(); + ao_gps_skip_field(); /* sats in view */ + while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') { + i = ao_gps_tracking_next.channels; + c = ao_gps_decimal(2); /* SVID */ + if (i < AO_MAX_GPS_TRACKING) + ao_gps_tracking_next.sats[i].svid = c; + ao_gps_lexchar(); + ao_gps_skip_field(); /* elevation */ + ao_gps_lexchar(); + 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) != 0) + ao_gps_tracking_next.channels = i + 1; } - if (i != AO_GPS_LEADER) - continue; + } + 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; + 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_mutex_put(&ao_gps_mutex); + ao_wakeup(&ao_gps_tracking_data); + } +} - /* pull the record identifier characters off the link */ - ao_gps_lexchar(); - a = ao_gps_char; +static void +ao_nmea_rmc(void) +{ + char a, c; + uint8_t i; + /* Parse the RMC record to read out the current date */ + + /* $GPRMC,111636.932,A,2447.0949,N,12100.5223,E,000.0,000.0,030407,,,A*61 + * + * Recommended Minimum Specific GNSS Data + * + * 111636.932 UTC time 11:16:36.932 + * A Data Valid (V = receiver warning) + * 2447.0949 Latitude + * N North/south indicator + * 12100.5223 Longitude + * E East/west indicator + * 000.0 Speed over ground + * 000.0 Course over ground + * 030407 UTC date (ddmmyy format) + * A Mode indicator: + * N = data not valid + * A = autonomous mode + * D = differential mode + * E = estimated (dead reckoning) mode + * M = manual input mode + * S = simulator mode + * 61 checksum + */ + ao_gps_skip_field(); + for (i = 0; i < 8; i++) { ao_gps_lexchar(); - c = ao_gps_char; + ao_gps_skip_field(); + } + 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(); - i = ao_gps_char; + 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; + if (!ao_gps_error) { + ao_gps_next.year = i; + ao_gps_next.month = c; + ao_gps_next.day = a; + ao_gps_date_flags = AO_GPS_DATE_VALID; + } +} + +#define ao_skytraq_sendstruct(s) ao_skytraq_sendbytes((s), (s)+sizeof(s)) + +static void +ao_skytraq_sendbytes(const uint8_t *b, const uint8_t *e) +{ + while (b != e) { + if (*b == 0xa0) + ao_delay(AO_MS_TO_TICKS(500)); + ao_serial_putchar(*b++); + } +} + +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 != ',') - continue; - - if (a == (uint8_t) 'G' && c == (uint8_t) 'G' && i == (uint8_t) 'A') { - /* Now read the data into the gps data record - * - * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66 - * - * Essential fix data - * - * 025149.000 time (02:51:49.000 GMT) - * 4528.1723,N Latitude 45°28.1723' N - * 12244.2480,W Longitude 122°44.2480' W - * 1 Fix quality: - * 0 = invalid - * 1 = GPS fix (SPS) - * 2 = DGPS fix - * 3 = PPS fix - * 4 = Real Time Kinematic - * 5 = Float RTK - * 6 = estimated (dead reckoning) - * 7 = Manual input mode - * 8 = Simulation mode - * 05 Number of satellites (5) - * 2.0 Horizontal dilution - * 103.5,M Altitude, 103.5M above msl - * -19.5,M Height of geoid above WGS84 ellipsoid - * ? time in seconds since last DGPS update - * 0000 DGPS station ID - * *66 checksum - */ - - ao_gps_next_tick = ao_time(); - ao_gps_next.flags = AO_GPS_RUNNING | ao_gps_date_flags; - ao_gps_next.hour = ao_gps_decimal(2); - ao_gps_next.minute = ao_gps_decimal(2); - ao_gps_next.second = ao_gps_decimal(2); - ao_gps_skip_field(); /* skip seconds fraction */ - - ao_gps_next.latitude = ao_gps_parse_pos(2); - if (ao_gps_parse_flag('N', 'S')) - ao_gps_next.latitude = -ao_gps_next.latitude; - ao_gps_next.longitude = ao_gps_parse_pos(3); - if (ao_gps_parse_flag('E', 'W')) - ao_gps_next.longitude = -ao_gps_next.longitude; - - i = ao_gps_decimal(0xff); - if (i == 1) - ao_gps_next.flags |= AO_GPS_VALID; - - i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT; - if (i > AO_GPS_NUM_SAT_MASK) - i = AO_GPS_NUM_SAT_MASK; - 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; - if (ao_gps_char == '.') - ao_gps_next.hdop = (ao_gps_next.hdop + - ((uint8_t) ao_gps_decimal(1) >> 1)); - } else - ao_gps_next.hdop = 255; - ao_gps_skip_field(); - - 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; - 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)); - ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_data); - } - } else if (a == (uint8_t) 'G' && c == (uint8_t) 'S' && i == (uint8_t) 'V') { - uint8_t done; - /* Now read the data into the GPS tracking data record - * - * $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72 - * - * Satellites in view data - * - * 3 Total number of GSV messages - * 1 Sequence number of current GSV message - * 12 Total sats in view (0-12) - * 05 SVID - * 54 Elevation - * 069 Azimuth - * 45 C/N0 in dB - * ... other SVIDs - * 72 checksum - */ - c = ao_gps_decimal(1); /* total messages */ - i = ao_gps_decimal(1); /* message sequence */ - if (i == 1) { - ao_gps_tracking_next.channels = 0; - } - done = (uint8_t) c == i; - ao_gps_lexchar(); - ao_gps_skip_field(); /* sats in view */ - while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') { - i = ao_gps_tracking_next.channels; - ao_gps_tracking_next.sats[i].svid = ao_gps_decimal(2); /* SVID */ - ao_gps_lexchar(); - ao_gps_skip_field(); /* elevation */ - ao_gps_lexchar(); - ao_gps_skip_field(); /* azimuth */ - if (!(ao_gps_tracking_next.sats[i].c_n_1 = ao_gps_decimal(2))) /* C/N0 */ - ao_gps_tracking_next.sats[i].svid = 0; - 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; - 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_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_tracking_data); - } - } else if (a == (uint8_t) 'R' && c == (uint8_t) 'M' && i == (uint8_t) 'C') { - /* Parse the RMC record to read out the current date */ - - /* $GPRMC,111636.932,A,2447.0949,N,12100.5223,E,000.0,000.0,030407,,,A*61 - * - * Recommended Minimum Specific GNSS Data - * - * 111636.932 UTC time 11:16:36.932 - * A Data Valid (V = receiver warning) - * 2447.0949 Latitude - * N North/south indicator - * 12100.5223 Longitude - * E East/west indicator - * 000.0 Speed over ground - * 000.0 Course over ground - * 030407 UTC date (ddmmyy format) - * A Mode indicator: - * N = data not valid - * A = autonomous mode - * D = differential mode - * E = estimated (dead reckoning) mode - * M = manual input mode - * S = simulator mode - * 61 checksum - */ - ao_gps_skip_field(); - for (i = 0; i < 8; i++) { - ao_gps_lexchar(); - ao_gps_skip_field(); - } - 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; - if (!ao_gps_error) { - ao_gps_next.year = i; - ao_gps_next.month = c; - ao_gps_next.day = a; - ao_gps_date_flags = AO_GPS_DATE_VALID; - } + 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 +{ + ao_serial_set_speed(AO_SERIAL_SPEED_9600); + + /* give skytraq time to boot in case of cold start */ + ao_delay(AO_MS_TO_TICKS(2000)); + + ao_skytraq_sendstruct(ao_gps_config); + + for (;;) { + /* Locate the begining of the next record */ + if (ao_serial_getchar() == '$') { + ao_gps_nmea_parse(); } + } } @@ -417,18 +460,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