#include "ao.h"
#endif
-#define AO_GPS_LEADER 2
+#define AO_GPS_LEADER 2
static const char ao_gps_header[] = "GP";
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,
+#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
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)
{
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<CR><LF>
+ *
+ * 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.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_skip_field(); /* Horizontal dilution */
-
- 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);
- 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<CR><LF>
- *
- * 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();
}
+
}
}
static void
gps_dump(void) __reentrant
{
+ uint8_t i;
ao_mutex_get(&ao_gps_mutex);
- ao_gps_print(&ao_gps_data);
- putchar('\n');
- ao_gps_tracking_print(&ao_gps_tracking_data);
- putchar('\n');
+ 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
projects / fw / altos / blobdiff