X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_gps_skytraq.c;h=c822f7fab57bf2125922f3fac5263369d74dcaf2;hp=b397d975340039e58c279baa1f929d9e06206376;hb=aa6c27df5db6bdae59d00affccb891854a6caa18;hpb=33b0b6f2f2e07de105619a7b463226d2813152ab

diff --git a/src/ao_gps_skytraq.c b/src/ao_gps_skytraq.c
index b397d975..c822f7fa 100644
--- a/src/ao_gps_skytraq.c
+++ b/src/ao_gps_skytraq.c
@@ -19,19 +19,22 @@
 #include "ao.h"
 #endif
 
-#define AO_GPS_LEADER		3
+#define AO_GPS_LEADER		2
 
-static const char ao_gps_header[] = "GPG";
+static const 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[] = {
@@ -46,6 +49,12 @@ static const char ao_gps_config[] = {
 	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 void
@@ -175,7 +184,7 @@ ao_gps_parse_flag(char no_c, char yes_c) __reentrant
 void
 ao_gps(void) __reentrant
 {
-	char	c;
+	char	a, c;
 	uint8_t	i;
 
 	ao_serial_set_speed(AO_SERIAL_SPEED_9600);
@@ -192,7 +201,7 @@ ao_gps(void) __reentrant
 		ao_gps_cksum = 0;
 		ao_gps_error = 0;
 
-		/* Skip anything other than GPG */
+		/* Skip anything other than GP */
 		for (i = 0; i < AO_GPS_LEADER; i++) {
 			ao_gps_lexchar();
 			if (ao_gps_char != ao_gps_header[i])
@@ -203,6 +212,8 @@ ao_gps(void) __reentrant
 
 		/* 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;
@@ -210,7 +221,7 @@ ao_gps(void) __reentrant
 		if (ao_gps_char != ',')
 			continue;
 
-		if (c == (uint8_t) 'G' && i == (uint8_t) 'A') {
+		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
@@ -239,7 +250,8 @@ ao_gps(void) __reentrant
 			 *	   *66		checksum
 			 */
 
-			ao_gps_next.flags = AO_GPS_RUNNING;
+			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);
@@ -262,7 +274,15 @@ ao_gps(void) __reentrant
 			ao_gps_next.flags |= i;
 
 			ao_gps_lexchar();
-			ao_gps_skip_field();	/* Horizontal dilution */
+			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 */
@@ -280,11 +300,12 @@ ao_gps(void) __reentrant
 				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 (c == (uint8_t) 'S' && i == (uint8_t) 'V') {
+		} 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
 			 *
@@ -317,10 +338,8 @@ ao_gps(void) __reentrant
 				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].state = 0xbf;
-				else
-					ao_gps_tracking_next.sats[i].state = 0;
+				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 == '*') {
@@ -339,6 +358,56 @@ ao_gps(void) __reentrant
 				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;
+			}
 		}
 	}
 }
@@ -349,16 +418,17 @@ static void
 gps_dump(void) __reentrant
 {
 	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);
 	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 },
+	{ 'g', gps_dump, 	"g                                  Display current GPS values" },
+	{ 0,   gps_dump, NULL },
 };
 
 void