diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 735079a20b..cec0dab3d6 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -83,6 +83,11 @@ AP_GPS_UBLOX::_request_next_config(void) _next_message--; } break; + case STEP_TIMEGPS: + if(!_request_message_rate(CLASS_NAV, MSG_TIMEGPS)) { + _next_message--; + } + break; case STEP_PORT: _request_port(); break; @@ -217,10 +222,11 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) { } break; case MSG_SOL: - if(rate == RATE_SOL) { + desired_rate = havePvtMsg ? 0 : RATE_SOL; + if(rate == desired_rate) { _unconfigured_messages &= ~CONFIG_RATE_SOL; } else { - _configure_message_rate(msg_class, msg_id, RATE_SOL); + _configure_message_rate(msg_class, msg_id, desired_rate); _unconfigured_messages |= CONFIG_RATE_SOL; _cfg_needs_save = true; } @@ -234,6 +240,15 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) { _cfg_needs_save = true; } break; + case MSG_TIMEGPS: + if(rate == RATE_TIMEGPS) { + _unconfigured_messages &= ~CONFIG_RATE_TIMEGPS; + } else { + _configure_message_rate(msg_class, msg_id, RATE_TIMEGPS); + _unconfigured_messages |= CONFIG_RATE_TIMEGPS; + _cfg_needs_save = true; + } + break; case MSG_VELNED: desired_rate = havePvtMsg ? 0 : RATE_VELNED; if(rate == desired_rate) { @@ -1017,6 +1032,11 @@ AP_GPS_UBLOX::_parse_gps(void) next_fix = state.status; #endif break; + case MSG_TIMEGPS: + Debug("MSG_TIMEGPS"); + if (_buffer.timegps.valid & UBX_TIMEGPS_VALID_WEEK_MASK) + state.time_week = _buffer.timegps.week; + break; case MSG_VELNED: Debug("MSG_VELNED"); if (havePvtMsg) { @@ -1278,8 +1298,8 @@ static const char *reasons[] = {"navigation rate", "navigation settings", "GNSS settings", "SBAS settings", - "PVT rate"}; - + "PVT rate", + "TIMEGPS rate"}; void AP_GPS_UBLOX::broadcast_configuration_failure_reason(void) const { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index d4024ec247..c9dd5b8ce3 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -47,11 +47,14 @@ #define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7 #define UBX_MSG_TYPES 2 +#define UBX_TIMEGPS_VALID_WEEK_MASK 0x2 + #define UBLOX_MAX_PORTS 6 #define RATE_POSLLH 1 #define RATE_STATUS 1 #define RATE_SOL 1 +#define RATE_TIMEGPS 5 #define RATE_PVT 1 #define RATE_VELNED 1 #define RATE_DOP 1 @@ -72,6 +75,8 @@ #define CONFIG_GNSS (1<<11) #define CONFIG_SBAS (1<<12) #define CONFIG_RATE_PVT (1<<13) +#define CONFIG_RATE_TIMEGPS (1<<14) +#define CONFIG_LAST (1<<15) // this must always be the last bit #define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED) @@ -274,6 +279,15 @@ private: uint32_t heading_accuracy; }; + struct PACKED ubx_nav_timegps { + uint32_t itow; + int32_t ftow; + uint16_t week; + int8_t leapS; + uint8_t valid; // leapsvalid | weekvalid | tow valid; + uint32_t tAcc; + }; + // Lea6 uses a 60 byte message struct PACKED ubx_mon_hw_60 { uint32_t pinSel; @@ -399,6 +413,7 @@ private: ubx_nav_dop dop; ubx_nav_solution solution; ubx_nav_pvt pvt; + ubx_nav_timegps timegps; ubx_nav_velned velned; ubx_cfg_msg_rate msg_rate; ubx_cfg_msg_rate_6 msg_rate_6; @@ -436,6 +451,7 @@ private: MSG_DOP = 0x4, MSG_SOL = 0x6, MSG_PVT = 0x7, + MSG_TIMEGPS = 0x20, MSG_VELNED = 0x12, MSG_CFG_CFG = 0x09, MSG_CFG_RATE = 0x08, @@ -490,6 +506,7 @@ private: STEP_STATUS, STEP_POSLLH, STEP_VELNED, + STEP_TIMEGPS, STEP_POLL_SVINFO, // poll svinfo STEP_POLL_SBAS, // poll SBAS STEP_POLL_NAV, // poll NAV settings