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

diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c
index f5268aa7..2556d314 100644
--- a/src/drivers/ao_gps_ublox.c
+++ b/src/drivers/ao_gps_ublox.c
@@ -28,7 +28,7 @@
 
 uint8_t ao_gps_new;
 uint8_t ao_gps_mutex;
-uint16_t ao_gps_tick;
+AO_TICK_TYPE ao_gps_tick;
 struct ao_telemetry_location	ao_gps_data;
 struct ao_telemetry_satellite	ao_gps_tracking_data;
 
@@ -615,6 +615,8 @@ 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();
 
@@ -645,6 +647,8 @@ ao_gps(void)
 		/* 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;
 
@@ -657,7 +661,7 @@ ao_gps(void)
 		ao_ublox_len = header_byte();
 		ao_ublox_len |= header_byte() << 8;
 
-		ao_gps_dbg(DBG_PROTO, "class %02x id %02x len %d\n", class, id, ao_ublox_len);
+		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;
@@ -679,6 +683,7 @@ ao_gps(void)
 				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)
@@ -715,7 +720,7 @@ ao_gps(void)
 			switch (id) {
 			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;
@@ -783,12 +788,12 @@ ao_gps(void)
 #if AO_UBLOX_DEBUG
 static void ao_gps_option(void)
 {
-	ao_cmd_hex();
+	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 = ao_cmd_lex_i;
+		ao_gps_dbg_enable = r;
 		printf ("gps debug set to %d\n", ao_gps_dbg_enable);
 	}
 }