X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_ublox.c;h=57cbf22a77b50245975955925096ba85ac9723f5;hb=fadb563f8aeeee895a7232bb71fa5bade6ef66f0;hp=a6d930835baed49a70d619073b84349242b0a806;hpb=c417ab1de2a083b5fcff2e081e4feb2a65887903;p=fw%2Faltos

diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c
index a6d93083..57cbf22a 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;
 
@@ -115,8 +115,8 @@ static void ao_ublox_put_i8(int8_t c)
 
 static void ao_ublox_put_u16(uint16_t c)
 {
-	ao_ublox_put_u8(c);
-	ao_ublox_put_u8(c>>8);
+	ao_ublox_put_u8((uint8_t) c);
+	ao_ublox_put_u8((uint8_t) (c>>8));
 }
 
 #if 0
@@ -128,10 +128,10 @@ static void ao_ublox_put_i16(int16_t c)
 
 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);
+	ao_ublox_put_u8((uint8_t) c);
+	ao_ublox_put_u8((uint8_t) (c>>8));
+	ao_ublox_put_u8((uint8_t) (c>>16));
+	ao_ublox_put_u8((uint8_t) (c>>24));
 }
 
 static void ao_ublox_put_i32(int32_t c)
@@ -160,7 +160,7 @@ static void ublox_u16(uint8_t offset)
 	uint16_t val;
 
 	val = data_byte();
-	val |= data_byte () << 8;
+	val |= (uint16_t) ((uint16_t) data_byte () << 8);
 	*ptr = val;
 }
 
@@ -515,8 +515,8 @@ ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
 	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);
+	ao_ublox_put_u8((uint8_t) len);
+	ao_ublox_put_u8((uint8_t) (len >> 8));
 }
 
 static void
@@ -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;
 
@@ -655,9 +659,9 @@ ao_gps(void)
 
 		/* Length */
 		ao_ublox_len = header_byte();
-		ao_ublox_len |= header_byte() << 8;
+		ao_ublox_len |= (uint16_t) ((uint16_t) 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;
@@ -733,7 +738,7 @@ ao_gps(void)
 				ao_gps_data.latitude = nav_posllh.lat;
 				ao_gps_data.longitude = nav_posllh.lon;
 
-				ao_gps_data.year = nav_timeutc.year - 2000;
+				ao_gps_data.year = (uint8_t) (nav_timeutc.year - 2000);
 				ao_gps_data.month = nav_timeutc.month;
 				ao_gps_data.day = nav_timeutc.day;
 
@@ -743,13 +748,13 @@ ao_gps(void)
 
 				/* 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.pdop = (uint8_t) (nav_dop.pdop / 10);
+				ao_gps_data.hdop = (uint8_t) (nav_dop.hdop / 10);
+				ao_gps_data.vdop = (uint8_t) (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_data.ground_speed = (uint16_t) nav_velned.g_speed;
+				ao_gps_data.climb_rate = -(int16_t) nav_velned.vel_d;
+				ao_gps_data.course = (uint8_t) (nav_velned.heading / 200000);
 
 				ao_gps_tracking_data.channels = 0;