X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_ublox.c;h=2556d314547b08cd91a2a3971bf0166a7697a5ff;hp=574b6a2c5686d0313f6e05b9d3d59d264dea8ea7;hb=7f46240dfc57164f0c1b0c4c4ed9695bca63860d;hpb=fb0fb6f4beab484e7fe55b39d18c1f19778f1211

diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c
index 574b6a2c..2556d314 100644
--- a/src/drivers/ao_gps_ublox.c
+++ b/src/drivers/ao_gps_ublox.c
@@ -3,7 +3,8 @@
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -21,31 +22,73 @@
 
 #include "ao_gps_ublox.h"
 
-__xdata uint8_t ao_gps_mutex;
-__pdata uint16_t ao_gps_tick;
-__xdata struct ao_telemetry_location	ao_gps_data;
-__xdata struct ao_telemetry_satellite	ao_gps_tracking_data;
+#define AO_UBLOX_DEBUG 0
 
-static const char ao_gps_set_nmea[] = "\r\n$PUBX,41,1,3,1,57600,0*2d\r\n";
+#include <stdarg.h>
 
-const char ao_gps_config[] = {
+uint8_t ao_gps_new;
+uint8_t ao_gps_mutex;
+AO_TICK_TYPE ao_gps_tick;
+struct ao_telemetry_location	ao_gps_data;
+struct ao_telemetry_satellite	ao_gps_tracking_data;
 
-};
+#undef AO_SERIAL_SPEED_UBLOX
+
+#ifndef AO_SERIAL_SPEED_UBLOX
+#define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_9600
+#endif
+
+#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_57600
+#define SERIAL_SPEED_STRING	"57600"
+#define SERIAL_SPEED_CHECKSUM	"2d"
+#endif
+#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_19200
+#define SERIAL_SPEED_STRING	"19200"
+#define SERIAL_SPEED_CHECKSUM	"23"
+#endif
+#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_9600
+#define SERIAL_SPEED_STRING	"9600"
+#define SERIAL_SPEED_CHECKSUM	"16"
+#endif
+
+static const char ao_gps_set_nmea[] =
+	"\r\n$PUBX,41,1,3,1," SERIAL_SPEED_STRING ",0*" SERIAL_SPEED_CHECKSUM "\r\n";
 
 struct ao_ublox_cksum {
 	uint8_t	a, b;
 };
 
-static __pdata struct ao_ublox_cksum ao_ublox_cksum;
-static __pdata uint16_t ao_ublox_len;
+static struct ao_ublox_cksum ao_ublox_cksum;
+static uint16_t ao_ublox_len;
+
+#if AO_UBLOX_DEBUG
+
+static uint8_t ao_gps_dbg_enable;
+
+#define DBG_PROTO	1
+#define DBG_CHAR	2
+#define DBG_INIT	4
 
-#ifndef ao_ublox_getchar
-#define ao_ublox_getchar	ao_serial1_getchar
-#define ao_ublox_putchar	ao_serial1_putchar
-#define ao_ublox_set_speed	ao_serial1_set_speed
+static void ao_gps_dbg(int level, char *format, ...) {
+	va_list a;
+
+	if (level & ao_gps_dbg_enable) {
+		va_start(a, format);
+		vprintf(format, a);
+		va_end(a);
+		flush();
+	}
+}
+
+#else
+#define ao_gps_dbg(fmt, ...)
 #endif
 
-#define ao_ublox_byte()	((uint8_t) ao_ublox_getchar())
+static inline uint8_t ao_ublox_byte(void) {
+	uint8_t	c = (uint8_t) ao_gps_getchar();
+	ao_gps_dbg(DBG_CHAR, " %02x", c);
+	return c;
+}
 
 static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c)
 {
@@ -58,10 +101,42 @@ static void ao_ublox_init_cksum(void)
 	ao_ublox_cksum.a = ao_ublox_cksum.b = 0;
 }
 
-static void ao_ublox_putchar_cksum(uint8_t c)
+static void ao_ublox_put_u8(uint8_t c)
 {
 	add_cksum(&ao_ublox_cksum, c);
-	ao_ublox_putchar(c);
+	ao_gps_dbg(DBG_CHAR, " (%02x)", c);
+	ao_gps_putchar(c);
+}
+
+static void ao_ublox_put_i8(int8_t c)
+{
+	ao_ublox_put_u8((uint8_t) c);
+}
+
+static void ao_ublox_put_u16(uint16_t c)
+{
+	ao_ublox_put_u8(c);
+	ao_ublox_put_u8(c>>8);
+}
+
+#if 0
+static void ao_ublox_put_i16(int16_t c)
+{
+	ao_ublox_put_u16((uint16_t) c);
+}
+#endif
+
+static void ao_ublox_put_u32(uint32_t c)
+{
+	ao_ublox_put_u8(c);
+	ao_ublox_put_u8(c>>8);
+	ao_ublox_put_u8(c>>16);
+	ao_ublox_put_u8(c>>24);
+}
+
+static void ao_ublox_put_i32(int32_t c)
+{
+	ao_ublox_put_u32((uint32_t) c);
 }
 
 static uint8_t header_byte(void)
@@ -77,11 +152,11 @@ static uint8_t data_byte(void)
 	return header_byte();
 }
 
-static char __xdata *ublox_target;
+static char *ublox_target;
 
 static void ublox_u16(uint8_t offset)
 {
-	uint16_t __xdata *ptr = (uint16_t __xdata *) (ublox_target + offset);
+	uint16_t *ptr = (uint16_t *) (void *) (ublox_target + offset);
 	uint16_t val;
 
 	val = data_byte();
@@ -91,16 +166,16 @@ static void ublox_u16(uint8_t offset)
 
 static void ublox_u8(uint8_t offset)
 {
-	uint8_t __xdata *ptr = (uint8_t __xdata *) (ublox_target + offset);
+	uint8_t *ptr = (uint8_t *) (ublox_target + offset);
 	uint8_t val;
 
 	val = data_byte ();
 	*ptr = val;
 }
 
-static void ublox_u32(uint8_t offset) __reentrant
+static void ublox_u32(uint8_t offset) 
 {
-	uint32_t __xdata *ptr = (uint32_t __xdata *) (ublox_target + offset);
+	uint32_t *ptr = (uint32_t *) (void *) (ublox_target + offset);
 	uint32_t val;
 
 	val = ((uint32_t) data_byte ());
@@ -128,7 +203,7 @@ struct ublox_packet_parse {
 };
 
 static void
-ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __reentrant
+ao_ublox_parse(void *target, const struct ublox_packet_parse *parse) 
 {
 	uint8_t	i, offset;
 
@@ -152,6 +227,7 @@ ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __r
 			break;
 		}
 	}
+	ao_gps_dbg(DBG_PROTO, "\n");
 }
 
 /*
@@ -295,6 +371,7 @@ ao_ublox_parse_nav_svinfo(void)
 {
 	uint8_t	nsat;
 	nav_svinfo_nsat = 0;
+
 	ao_ublox_parse(&nav_svinfo, nav_svinfo_packet);
 	for (nsat = 0; nsat < nav_svinfo.num_ch && ao_ublox_len >= 12; nsat++) {
 		if (nsat < NAV_SVINFO_MAX_SAT) {
@@ -303,6 +380,17 @@ ao_ublox_parse_nav_svinfo(void)
 			ublox_discard(12);
 		}
 	}
+#if AO_UBLOX_DEBUG
+	ao_gps_dbg(DBG_PROTO, "svinfo num_ch %d flags %02x\n", nav_svinfo.num_ch, nav_svinfo.flags);
+	for (nsat = 0; nsat < nav_svinfo.num_ch; nsat++)
+		ao_gps_dbg(DBG_PROTO, "\t%d: chn %d svid %d flags %02x quality %d cno %d\n",
+			    nsat,
+			    nav_svinfo_sat[nsat].chn,
+			    nav_svinfo_sat[nsat].svid,
+			    nav_svinfo_sat[nsat].flags,
+			    nav_svinfo_sat[nsat].quality,
+			    nav_svinfo_sat[nsat].cno);
+#endif
 }
 
 /*
@@ -371,68 +459,119 @@ ao_ublox_parse_nav_velned(void)
  */
 
 static void
-ao_gps_setup(void)
+ao_gps_delay(void)
 {
-	uint8_t	i, k;
-	ao_ublox_set_speed(AO_SERIAL_SPEED_9600);
+	uint8_t i;
 
 	/*
 	 * A bunch of nulls so the start bit
 	 * is clear
 	 */
+
 	for (i = 0; i < 64; i++)
-		ao_ublox_putchar(0x00);
+		ao_gps_putchar(0x00);
+}
+
+static void
+ao_gps_setup(void)
+{
+	uint8_t	i, k;
+
+	ao_delay(AO_SEC_TO_TICKS(3));
+
+	ao_gps_dbg(DBG_INIT, "Set speed 9600\n");
+	ao_gps_set_speed(AO_SERIAL_SPEED_9600);
 
 	/*
 	 * Send the baud-rate setting and protocol-setting
 	 * command three times
 	 */
-	for (k = 0; k < 3; k++)
+	for (k = 0; k < 3; k++) {
+		ao_gps_delay();
+
+		ao_gps_dbg(DBG_INIT, "Send initial setting\n");
 		for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
-			ao_ublox_putchar(ao_gps_set_nmea[i]);
+			ao_gps_putchar(ao_gps_set_nmea[i]);
+	}
+
+	ao_gps_delay();
 
+#if AO_SERIAL_SPEED_UBLOX != AO_SERIAL_SPEED_9600
+	ao_gps_dbg(DBG_INIT, "Set speed high\n");
 	/*
 	 * Increase the baud rate
 	 */
-	ao_ublox_set_speed(AO_SERIAL_SPEED_57600);
+	ao_gps_set_speed(AO_SERIAL_SPEED_UBLOX);
+#endif
 
-	/*
-	 * Pad with nulls to give the chip
-	 * time to see the baud rate switch
-	 */
-	for (i = 0; i < 64; i++)
-		ao_ublox_putchar(0x00);
+	ao_gps_delay();
 }
 
-void
+static void
 ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
 {
 	ao_ublox_init_cksum();
-	ao_ublox_putchar(0xb5);
-	ao_ublox_putchar(0x62);
-	ao_ublox_putchar_cksum(class);
-	ao_ublox_putchar_cksum(id);
-	ao_ublox_putchar_cksum(len);
-	ao_ublox_putchar_cksum(len >> 8);
+	ao_gps_putchar(0xb5);
+	ao_gps_putchar(0x62);
+	ao_ublox_put_u8(class);
+	ao_ublox_put_u8(id);
+	ao_ublox_put_u8(len);
+	ao_ublox_put_u8(len >> 8);
 }
 
-void
+static void
 ao_ublox_putend(void)
 {
-	ao_ublox_putchar(ao_ublox_cksum.a);
-	ao_ublox_putchar(ao_ublox_cksum.b);
+	ao_gps_putchar(ao_ublox_cksum.a);
+	ao_gps_putchar(ao_ublox_cksum.b);
 }
 
-void
+static void
 ao_ublox_set_message_rate(uint8_t class, uint8_t msgid, uint8_t rate)
 {
 	ao_ublox_putstart(0x06, 0x01, 3);
-	ao_ublox_putchar_cksum(class);
-	ao_ublox_putchar_cksum(msgid);
-	ao_ublox_putchar_cksum(rate);
+	ao_ublox_put_u8(class);
+	ao_ublox_put_u8(msgid);
+	ao_ublox_put_u8(rate);
 	ao_ublox_putend();
 }
 
+static void
+ao_ublox_set_navigation_settings(uint16_t mask,
+				 uint8_t dyn_model,
+				 uint8_t fix_mode,
+				 int32_t fixed_alt,
+				 uint32_t fixed_alt_var,
+				 int8_t min_elev,
+				 uint8_t dr_limit,
+				 uint16_t pdop,
+				 uint16_t tdop,
+				 uint16_t pacc,
+				 uint16_t tacc,
+				 uint8_t static_hold_thresh,
+				 uint8_t dgps_time_out)
+{
+	ao_ublox_putstart(UBLOX_CFG, UBLOX_CFG_NAV5, 36);
+	ao_ublox_put_u16(mask);
+	ao_ublox_put_u8(dyn_model);
+	ao_ublox_put_u8(fix_mode);
+	ao_ublox_put_i32(fixed_alt);
+	ao_ublox_put_u32(fixed_alt_var);
+	ao_ublox_put_i8(min_elev);
+	ao_ublox_put_u8(dr_limit);
+	ao_ublox_put_u16(pdop);
+	ao_ublox_put_u16(tdop);
+	ao_ublox_put_u16(pacc);
+	ao_ublox_put_u16(tacc);
+	ao_ublox_put_u8(static_hold_thresh);
+	ao_ublox_put_u8(dgps_time_out);
+	ao_ublox_put_u32(0);
+	ao_ublox_put_u32(0);
+	ao_ublox_put_u32(0);
+	ao_ublox_putend();
+}
+
+
 /*
  * Disable all MON message
  */
@@ -463,11 +602,21 @@ static const uint8_t ublox_enable_nav[] = {
 };
 
 void
-ao_gps(void) __reentrant
+ao_gps_set_rate(uint8_t rate)
+{
+	uint8_t	i;
+	for (i = 0; i < sizeof (ublox_enable_nav); i++)
+		ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], rate);
+}
+
+void
+ao_gps(void) 
 {
 	uint8_t			class, id;
 	struct ao_ublox_cksum	cksum;
 	uint8_t			i;
+	AO_TICK_TYPE		packet_start_tick;
+	AO_TICK_TYPE		solution_tick = 0;
 
 	ao_gps_setup();
 
@@ -478,13 +627,28 @@ ao_gps(void) __reentrant
 		ao_ublox_set_message_rate(UBLOX_NAV, ublox_disable_nav[i], 0);
 
 	/* Enable all of the messages we want */
-	for (i = 0; i < sizeof (ublox_enable_nav); i++)
-		ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], 1);
-	
+	ao_gps_set_rate(1);
+
+	ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE),
+					 UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G,
+					 UBLOX_CFG_NAV5_FIXMODE_3D,
+					 0,
+					 0,
+					 0,
+					 0,
+					 0,
+					 0,
+					 0,
+					 0,
+					 0,
+					 0);
+
 	for (;;) {
 		/* Locate the begining of the next record */
 		while (ao_ublox_byte() != (uint8_t) 0xb5)
 			;
+		packet_start_tick = ao_tick_count;
+
 		if (ao_ublox_byte() != (uint8_t) 0x62)
 			continue;
 
@@ -497,6 +661,8 @@ ao_gps(void) __reentrant
 		ao_ublox_len = header_byte();
 		ao_ublox_len |= header_byte() << 8;
 
+		ao_gps_dbg(DBG_PROTO, "%6u class %02x id %02x len %d\n", packet_start_tick, class, id, ao_ublox_len);
+
 		if (ao_ublox_len > 1023)
 			continue;
 
@@ -517,6 +683,7 @@ ao_gps(void) __reentrant
 				if (ao_ublox_len != 52)
 					break;
 				ao_ublox_parse_nav_sol();
+				solution_tick = packet_start_tick;
 				break;
 			case UBLOX_NAV_SVINFO:
 				if (ao_ublox_len < 8)
@@ -537,8 +704,10 @@ ao_gps(void) __reentrant
 			break;
 		}
 
-		if (ao_ublox_len != 0)
+		if (ao_ublox_len != 0) {
+			ao_gps_dbg(DBG_PROTO, "len left %d\n", ao_ublox_len);
 			continue;
+		}
 
 		/* verify checksum and end sequence */
 		cksum.a = ao_ublox_byte();
@@ -547,25 +716,25 @@ ao_gps(void) __reentrant
 			continue;
 
 		switch (class) {
-		case 0x01:
+		case UBLOX_NAV:
 			switch (id) {
-			case 0x21:
+			case UBLOX_NAV_TIMEUTC:
 				ao_mutex_get(&ao_gps_mutex);
-				ao_gps_tick = ao_time();
+				ao_gps_tick = solution_tick;
 
 				ao_gps_data.flags = 0;
 				ao_gps_data.flags |= AO_GPS_RUNNING;
 				if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) {
 					uint8_t	nsat = nav_sol.nsat;
-					ao_gps_data.flags |= AO_GPS_VALID;
+					ao_gps_data.flags |= AO_GPS_VALID | AO_GPS_COURSE_VALID;
 					if (nsat > 15)
 						nsat = 15;
 					ao_gps_data.flags |= nsat;
 				}
 				if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC))
 					ao_gps_data.flags |= AO_GPS_DATE_VALID;
-				
-				ao_gps_data.altitude = nav_posllh.alt_msl / 1000;
+
+				AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, nav_posllh.alt_msl / 1000);
 				ao_gps_data.latitude = nav_posllh.lat;
 				ao_gps_data.longitude = nav_posllh.lon;
 
@@ -577,36 +746,38 @@ ao_gps(void) __reentrant
 				ao_gps_data.minute = nav_timeutc.min;
 				ao_gps_data.second = nav_timeutc.sec;
 
-				ao_gps_data.pdop = nav_dop.pdop;
-				ao_gps_data.hdop = nav_dop.hdop;
-				ao_gps_data.vdop = nav_dop.vdop;
-
-				/* mode is not set */
+				/* we report dop scaled by 10, but ublox provides dop scaled by 100
+				 */
+				ao_gps_data.pdop = nav_dop.pdop / 10;
+				ao_gps_data.hdop = nav_dop.hdop / 10;
+				ao_gps_data.vdop = nav_dop.vdop / 10;
 
 				ao_gps_data.ground_speed = nav_velned.g_speed;
 				ao_gps_data.climb_rate = -nav_velned.vel_d;
 				ao_gps_data.course = nav_velned.heading / 200000;
-				
+
 				ao_gps_tracking_data.channels = 0;
 
 				struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0];
+				struct nav_svinfo_sat *src = &nav_svinfo_sat[0];
 
 				for (i = 0; i < nav_svinfo_nsat; i++) {
-					struct nav_svinfo_sat *src = &nav_svinfo_sat[i];
-
 					if (!(src->flags & (1 << NAV_SVINFO_SAT_FLAGS_UNHEALTHY)) &&
 					    src->quality >= NAV_SVINFO_SAT_QUALITY_ACQUIRED)
 					{
-						dst->svid = src->svid;
-						dst->c_n_1 = src->cno;
-						dst++;
-						ao_gps_tracking_data.channels++;
+						if (ao_gps_tracking_data.channels < AO_TELEMETRY_SATELLITE_MAX_SAT) {
+							dst->svid = src->svid;
+							dst->c_n_1 = src->cno;
+							dst++;
+							ao_gps_tracking_data.channels++;
+						}
 					}
+					src++;
 				}
 
 				ao_mutex_put(&ao_gps_mutex);
-				ao_wakeup(&ao_gps_data);
-				ao_wakeup(&ao_gps_tracking_data);
+				ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
+				ao_wakeup(&ao_gps_new);
 				break;
 			}
 			break;
@@ -614,10 +785,32 @@ ao_gps(void) __reentrant
 	}
 }
 
-__xdata struct ao_task ao_gps_task;
+#if AO_UBLOX_DEBUG
+static void ao_gps_option(void)
+{
+	uint16_t r = ao_cmd_hex();
+	if (ao_cmd_status != ao_cmd_success) {
+		ao_cmd_status = ao_cmd_success;
+		ao_gps_show();
+	} else {
+		ao_gps_dbg_enable = r;
+		printf ("gps debug set to %d\n", ao_gps_dbg_enable);
+	}
+}
+#else
+#define ao_gps_option ao_gps_show
+#endif
+
+const struct ao_cmds ao_gps_cmds[] = {
+	{ ao_gps_option, 	"g\0Display GPS" },
+	{ 0, NULL },
+};
+
+struct ao_task ao_gps_task;
 
 void
 ao_gps_init(void)
 {
+	ao_cmd_register(&ao_gps_cmds[0]);
 	ao_add_task(&ao_gps_task, ao_gps, "gps");
 }