Parse GPS data locally. Add 'g' command to display recent GPS results.

This parses the GPS GGA message and stores it in a global variable,
protected by a mutex.

3 files changed, 276 insertions, 30 deletions

diff --git a/ao.h b/ao.h
index 20c2458c..bbf358e9 100644
--- a/ao.h
+++ b/ao.h
@@ -477,15 +477,15 @@ enum ao_flight_state {
 	ao_flight_invalid
 };
 
-extern __xdata struct ao_adc	ao_flight_data;
-extern __data enum flight_state	ao_flight_state;
-extern __data uint16_t			ao_flight_tick;
-extern __data int16_t			ao_flight_accel;
-extern __data int16_t			ao_flight_pres;
-extern __data int16_t			ao_ground_pres;
-extern __data int16_t			ao_ground_accel;
-extern __data int16_t			ao_min_pres;
-extern __data uint16_t			ao_launch_time;
+extern __xdata struct ao_adc		ao_flight_data;
+extern __pdata enum flight_state	ao_flight_state;
+extern __pdata uint16_t			ao_flight_tick;
+extern __pdata int16_t			ao_flight_accel;
+extern __pdata int16_t			ao_flight_pres;
+extern __pdata int16_t			ao_ground_pres;
+extern __pdata int16_t			ao_ground_accel;
+extern __pdata int16_t			ao_min_pres;
+extern __pdata uint16_t			ao_launch_time;
 
 /* Flight thread */
 void
@@ -585,6 +585,37 @@ ao_serial_init(void);
  * ao_gps.c
  */
 
+struct ao_gps_pos {
+	uint8_t	degrees;
+	uint8_t	minutes;
+	uint16_t minutes_fraction;	/* in units of 1/10000 minutes */
+};
+
+#define AO_GPS_NUM_SAT_MASK	(0xf << 0)
+#define AO_GPS_NUM_SAT_SHIFT	(0)
+
+#define AO_GPS_VALID		(1 << 4)
+#define AO_GPS_LONGITUDE_MASK	(1 << 5)
+#define AO_GPS_LONGITUDE_EAST	(0 << 5)
+#define AO_GPS_LONGITUDE_WEST	(1 << 5)
+
+#define AO_GPS_LATITUDE_MASK	(1 << 6)
+#define AO_GPS_LATITUDE_NORTH	(0 << 6)
+#define AO_GPS_LATITUDE_SOUTH	(1 << 6)
+
+struct ao_gps_data {
+	uint8_t			hour;
+	uint8_t			minute;
+	uint8_t			second;
+	uint8_t			flags;
+	struct ao_gps_pos	latitude;
+	struct ao_gps_pos	longitude;
+	int16_t			altitude;
+};
+
+extern __xdata uint8_t ao_gps_mutex;
+extern __xdata struct ao_gps_data ao_gps_data;
+
 void
 ao_gps(void);
 
diff --git a/ao_cmd.c b/ao_cmd.c
index 575ed69a..6e91a72d 100644
--- a/ao_cmd.c
+++ b/ao_cmd.c
@@ -253,6 +253,34 @@ adc_dump(void)
 }
 
 static void
+gps_dump(void) __reentrant
+{
+	ao_mutex_get(&ao_gps_mutex);
+	if (ao_gps_data.flags & AO_GPS_VALID) {
+		printf("GPS %2d:%02d:%02d %2d°%2d.%04d'%c %2d°%2d.%04d'%c %5dm %2d sat\n",
+		       ao_gps_data.hour,
+		       ao_gps_data.minute,
+		       ao_gps_data.second,
+		       ao_gps_data.latitude.degrees,
+		       ao_gps_data.latitude.minutes,
+		       ao_gps_data.latitude.minutes_fraction,
+		       (ao_gps_data.flags & AO_GPS_LATITUDE_MASK) == AO_GPS_LATITUDE_NORTH ?
+		       'N' : 'S',
+		       ao_gps_data.longitude.degrees,
+		       ao_gps_data.longitude.minutes,
+		       ao_gps_data.longitude.minutes_fraction,
+		       (ao_gps_data.flags & AO_GPS_LONGITUDE_MASK) == AO_GPS_LONGITUDE_WEST ?
+		       'W' : 'E',
+		       ao_gps_data.altitude,
+		       (ao_gps_data.flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT);
+	} else {
+		printf("GPS %2d sat\n",
+		       (ao_gps_data.flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT);;
+	}
+	ao_mutex_put(&ao_gps_mutex);
+}
+
+static void
 dump(void)
 {
 	__xdata uint16_t c;
@@ -488,6 +516,7 @@ static const uint8_t help_txt[] =
 	"All numbers are in hex\n"
 	"?                                  Print this message\n"
 	"a                                  Display current ADC values\n"
+	"g                                  Display current GPS values\n"
 	"d <start> <end>                    Dump memory\n"
 	"e <block>                          Dump a block of EEPROM data\n"
 	"w <block> <start> <len> <data> ... Write data to EEPROM\n"
@@ -546,6 +575,9 @@ ao_cmd(void *parameters)
 		case 'a':
 			adc_dump();
 			break;
+		case 'g':
+			gps_dump();
+			break;
 		case 'e':
 			ee_dump();
 			break;
diff --git a/ao_gps.c b/ao_gps.c
index e689b704..83f44d55 100644
--- a/ao_gps.c
+++ b/ao_gps.c
@@ -19,13 +19,16 @@
 
 __xdata struct ao_task ao_gps_task;
 
-#define AO_GPS_MAX_LINE		80
-
 #define AO_GPS_LEADER		6
 
-__xdata uint8_t ao_gps_line[AO_GPS_MAX_LINE+1] = "GPGGA,";
+static const uint8_t ao_gps_header[] = "GPGGA,";
 __xdata uint8_t ao_gps_mutex;
-__xdata uint8_t ao_gps_data;
+static __xdata uint8_t ao_gps_char;
+static __xdata uint8_t ao_gps_cksum;
+static __xdata uint8_t ao_gps_error;
+
+__xdata struct ao_gps_data	ao_gps_data;
+static __xdata struct ao_gps_data	ao_gps_next;
 
 const uint8_t ao_gps_config[] =
 	"$PSRF103,00,00,01,01*25\r\n"	/* GGA 1 per sec */
@@ -35,9 +38,122 @@ const uint8_t ao_gps_config[] =
 	"$PSRF103,04,00,00,01*20\r\n"	/* RMC disable */
 	"$PSRF103,05,00,00,01*21\r\n";	/* VTG disable */
 
+static void
+ao_gps_lexchar(void)
+{
+	if (ao_gps_error)
+		ao_gps_char = '\n';
+	else 
+		ao_gps_char = ao_serial_getchar();
+	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)
+{
+	while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n')
+		ao_gps_lexchar();
+}
+
+void
+ao_gps_skip_sep(void)
+{
+	while (ao_gps_char == ',' || ao_gps_char == '.' || ao_gps_char == '*')
+		ao_gps_lexchar();
+}
+
+__xdata 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;
+	
+	ao_gps_skip_sep();
+	if (ao_gps_char == '-') {
+		neg = 1;
+		ao_gps_lexchar();
+	}
+	v = 0;
+	ao_gps_num_width = 0;
+	while (ao_gps_num_width < max_width) {
+		if (ao_gps_char < '0' || '9' < ao_gps_char)
+			break;
+		v = v * (int16_t) 10 + ao_gps_char - '0';
+		ao_gps_num_width++;
+		ao_gps_lexchar();
+	}
+	if (neg)
+		v = -v;
+	return v;
+}
+
+static uint8_t
+ao_gps_hex(uint8_t max_width)
+{
+	uint8_t	v, d;
+
+	ao_gps_skip_sep();
+	v = 0;
+	ao_gps_num_width = 0;
+	while (ao_gps_num_width < max_width) {
+		if ('0' <= ao_gps_char && ao_gps_char <= '9')
+			d = ao_gps_char - '0';
+		else if ('A' <= ao_gps_char && ao_gps_char <= 'F')
+			d = ao_gps_char - 'A' + 10;
+		else if ('a' <= ao_gps_char && ao_gps_char <= 'f')
+			d = ao_gps_char - 'a' + 10;
+		else
+			break;
+		v = (v << 4) | d;
+		ao_gps_num_width++;
+		ao_gps_lexchar();
+	}
+	return v;
+}
+
+static void
+ao_gps_parse_pos(__xdata struct ao_gps_pos * pos, uint8_t deg_width) __reentrant
+{
+	pos->degrees = ao_gps_decimal(deg_width);
+	pos->minutes = ao_gps_decimal(2);
+	if (ao_gps_char == '.') {
+		pos->minutes_fraction = ao_gps_decimal(4);
+		while (ao_gps_num_width < 4) {
+			pos->minutes_fraction *= 10;
+			ao_gps_num_width++;
+		}
+	} else {
+		pos->minutes_fraction = 0;
+		if (ao_gps_char != ',')
+			ao_gps_error = 1;
+	}
+}
+
+static void
+ao_gps_parse_flag(uint8_t yes_c, uint8_t yes, uint8_t no_c, uint8_t no) __reentrant
+{
+	ao_gps_skip_sep();
+	if (ao_gps_char == yes_c)
+		ao_gps_next.flags |= yes;
+	else if (ao_gps_char == no_c)
+		ao_gps_next.flags |= no;
+	else
+		ao_gps_error = 1;
+	ao_gps_lexchar();
+}
+
 
 void
-ao_gps(void)
+ao_gps(void) __reentrant
 {
 	uint8_t	c;
 	uint8_t	i;
@@ -45,35 +161,102 @@ ao_gps(void)
 	for (i = 0; (c = ao_gps_config[i]); i++)
 		ao_serial_putchar(c);
 	for (;;) {
+		/* Locate the begining of the next record */
 		for (;;) {
 			c = ao_serial_getchar();
 			if (c == '$')
 				break;
 		}
-		for (i = 0; i < AO_GPS_LEADER; i++)
-			if (ao_serial_getchar() != ao_gps_line[i])
+
+		ao_gps_cksum = 0;
+		ao_gps_error = 0;
+		
+		/* Skip anything other than GGA */
+		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_mutex_get(&ao_gps_mutex);
-		for (;;) {
-			c = ao_serial_getchar();
-			if (c < ' ')
-				break;
-			if (i < AO_GPS_MAX_LINE)
-				ao_gps_line[i++] = c;
+
+		/* 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 = 0;
+		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_parse_pos(&ao_gps_next.latitude, 2);
+		ao_gps_parse_flag('N', AO_GPS_LATITUDE_NORTH, 'S', AO_GPS_LATITUDE_SOUTH);
+		ao_gps_parse_pos(&ao_gps_next.longitude, 3);
+		ao_gps_parse_flag('W', AO_GPS_LONGITUDE_WEST, 'E', AO_GPS_LONGITUDE_EAST);
+		
+		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 == '*') {
+			c = ao_gps_cksum ^ '*';
+			i = ao_gps_hex(2);
+			if (c != i)
+				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 (ao_gps_data));
+			ao_mutex_put(&ao_gps_mutex);
+			ao_wakeup(&ao_gps_data);
 		}
-		ao_gps_line[i] = '\0';
-		ao_gps_data = 1;
-		ao_mutex_put(&ao_gps_mutex);
-		ao_wakeup(&ao_gps_data);
-		puts(ao_gps_line);
-		ao_usb_flush();
 	}
 }
 
 void
 ao_gps_init(void)
 {
-	ao_add_task(&ao_gps_task, ao_gps);
+	ao_add_task(&ao_gps_task, ao_gps, "gps");
 }