diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/ao_gps_skytraq.c | 146 | ||||
| -rw-r--r-- | src/ao_host.h | 1 |
2 files changed, 82 insertions, 65 deletions
diff --git a/src/ao_gps_skytraq.c b/src/ao_gps_skytraq.c index a2d5f1db..4d4ca592 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 @@ -68,13 +76,6 @@ ao_gps_lexchar(void) } void -ao_gps_skip(void) -{ - while (ao_gps_char >= '0') - ao_gps_lexchar(); -} - -void ao_gps_skip_field(void) { while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n') @@ -390,53 +391,68 @@ ao_nmea_rmc(void) } } +#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 != 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(); } + } } diff --git a/src/ao_host.h b/src/ao_host.h index fa03a910..a96b7629 100644 --- a/src/ao_host.h +++ b/src/ao_host.h @@ -69,6 +69,7 @@ uint8_t ao_adc_head; #define ao_cmd_register(c) #define ao_usb_disable() #define ao_telemetry_set_interval(x) +#define ao_delay(x) enum ao_igniter { ao_igniter_drogue = 0, |