Merge branch 'master'

[fw/altos] / src / drivers / ao_gps_ublox.c

diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c
index a6d930835baed49a70d619073b84349242b0a806..3d615e9c933f14b260f3cd0fb75faeec68073a3c 100644 (file)
--- a/src/drivers/ao_gps_ublox.c
+++ b/src/drivers/ao_gps_ublox.c
@@ -28,7 +28,8 @@
 
 uint8_t ao_gps_new;
 uint8_t ao_gps_mutex;
-uint16_t ao_gps_tick;
+AO_TICK_TYPE ao_gps_tick;
+AO_TICK_TYPE ao_gps_utc_tick;
 struct ao_telemetry_location   ao_gps_data;
 struct ao_telemetry_satellite  ao_gps_tracking_data;
 
@@ -63,12 +64,13 @@ 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
 
+static uint8_t ao_gps_dbg_enable = DBG_PROTO|DBG_CHAR|DBG_INIT;
+
 static void ao_gps_dbg(int level, char *format, ...) {
        va_list a;
 
@@ -115,8 +117,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 +130,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 +162,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;
 }
 
@@ -397,6 +399,7 @@ ao_ublox_parse_nav_svinfo(void)
  * NAV-TIMEUTC message parsing
  */
 static struct nav_timeutc {
+       int32_t         nano;
        uint16_t        year;
        uint8_t         month;
        uint8_t         day;
@@ -411,7 +414,8 @@ static struct nav_timeutc {
 #define NAV_TIMEUTC_VALID_UTC  2
 
 static const struct ublox_packet_parse nav_timeutc_packet[] = {
-       { UBLOX_DISCARD, 12 },                                          /* 0 iTOW, tAcc, nano */
+       { UBLOX_DISCARD, 8 },                                           /* 0 iTOW, tAcc */
+       { UBLOX_U32, offsetof(struct nav_timeutc, nano) },              /* 8 nano */
        { UBLOX_U16, offsetof(struct nav_timeutc, year) },              /* 12 year */
        { UBLOX_U8, offsetof(struct nav_timeutc, month) },              /* 14 month */
        { UBLOX_U8, offsetof(struct nav_timeutc, day) },                /* 15 day */
@@ -515,8 +519,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 +619,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 +651,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 +663,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 +687,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,8 +724,8 @@ 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_utc_tick = packet_start_tick + (AO_TICK_TYPE) AO_NS_TO_TICKS(nav_timeutc.nano);
                                ao_gps_data.flags = 0;
                                ao_gps_data.flags |= AO_GPS_RUNNING;
                                if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) {
@@ -733,7 +742,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 +752,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;
 
@@ -783,7 +792,7 @@ ao_gps(void)
 #if AO_UBLOX_DEBUG
 static void ao_gps_option(void)
 {
-       uint16_t r = ao_cmd_hex();
+       uint8_t r = (uint8_t) ao_cmd_hex();
        if (ao_cmd_status != ao_cmd_success) {
                ao_cmd_status = ao_cmd_success;
                ao_gps_show();