diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index ac6d1c0069..db445cc8a8 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -65,7 +65,8 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART _disable_counter(0), next_fix(AP_GPS::NO_FIX), _cfg_needs_save(false), - noReceivedHdop(true) + noReceivedHdop(true), + havePvtMsg(false) { // stop any config strings that are pending gps.send_blob_start(state.instance, nullptr, 0); @@ -95,12 +96,12 @@ AP_GPS_UBLOX::_request_next_config(void) _configure_rate(); break; case STEP_RATE_POSLLH: - if(!_configure_message_rate(CLASS_NAV, MSG_POSLLH, RATE_POSLLH)) { + if(!havePvtMsg && !_configure_message_rate(CLASS_NAV, MSG_POSLLH, RATE_POSLLH)) { _next_message--; } break; case STEP_RATE_VELNED: - if(!_configure_message_rate(CLASS_NAV, MSG_VELNED, RATE_VELNED)) { + if(!havePvtMsg && !_configure_message_rate(CLASS_NAV, MSG_VELNED, RATE_VELNED)) { _next_message--; } break; @@ -130,12 +131,12 @@ AP_GPS_UBLOX::_request_next_config(void) _send_message(CLASS_CFG, MSG_CFG_RATE, nullptr, 0); break; case STEP_POSLLH: - if(!_request_message_rate(CLASS_NAV, MSG_POSLLH)) { + if(!havePvtMsg && !_request_message_rate(CLASS_NAV, MSG_POSLLH)) { _next_message--; } break; case STEP_STATUS: - if(!_request_message_rate(CLASS_NAV, MSG_STATUS)) { + if(!havePvtMsg && !_request_message_rate(CLASS_NAV, MSG_STATUS)) { _next_message--; } break; @@ -144,8 +145,13 @@ AP_GPS_UBLOX::_request_next_config(void) _next_message--; } break; + case STEP_PVT: + if(!_request_message_rate(CLASS_NAV, MSG_PVT)) { + _next_message--; + } + break; case STEP_VELNED: - if(!_request_message_rate(CLASS_NAV, MSG_VELNED)) { + if(!havePvtMsg && !_request_message_rate(CLASS_NAV, MSG_VELNED)) { _next_message--; } break; @@ -204,6 +210,7 @@ AP_GPS_UBLOX::_request_next_config(void) void AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) { + Debug("class=%u id=%u rate=%u",msg_class,msg_id,rate); switch(msg_class) { case CLASS_NAV: switch(msg_id) { @@ -234,6 +241,15 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) { _cfg_needs_save = true; } break; + case MSG_PVT: + if(rate == RATE_PVT) { + _unconfigured_messages &= ~CONFIG_RATE_PVT; + } else { + _configure_message_rate(msg_class, msg_id, RATE_PVT); + _unconfigured_messages |= CONFIG_RATE_PVT; + _cfg_needs_save = true; + } + break; case MSG_VELNED: if(rate == RATE_VELNED) { _unconfigured_messages &= ~CONFIG_RATE_VELNED; @@ -342,7 +358,7 @@ AP_GPS_UBLOX::read(void) _delay_time = 750; } } else { - _delay_time = 2000; + _delay_time = 4000; } } @@ -823,6 +839,10 @@ AP_GPS_UBLOX::_parse_gps(void) switch (_msg_id) { case MSG_POSLLH: Debug("MSG_POSLLH next_fix=%u", next_fix); + if (havePvtMsg) { + _configure_message_rate(_class, _msg_id, 0); + break; + } _last_pos_time = _buffer.posllh.time; state.location.lng = _buffer.posllh.longitude; state.location.lat = _buffer.posllh.latitude; @@ -845,6 +865,10 @@ AP_GPS_UBLOX::_parse_gps(void) Debug("MSG_STATUS fix_status=%u fix_type=%u", _buffer.status.fix_status, _buffer.status.fix_type); + if (havePvtMsg) { + _configure_message_rate(_class, _msg_id, 0); + break; + } if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) { if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) && (_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) { @@ -880,6 +904,10 @@ AP_GPS_UBLOX::_parse_gps(void) Debug("MSG_SOL fix_status=%u fix_type=%u", _buffer.solution.fix_status, _buffer.solution.fix_type); + if (havePvtMsg) { + state.time_week = _buffer.solution.week; + break; + } if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) { if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) && (_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) { @@ -914,8 +942,88 @@ AP_GPS_UBLOX::_parse_gps(void) state.hdop = 130; #endif break; + case MSG_PVT: + Debug("MSG_PVT"); + havePvtMsg = true; + //unsubscribe to these messages + _unconfigured_messages &= ~CONFIG_RATE_POSLLH; + _unconfigured_messages &= ~CONFIG_RATE_VELNED; + _unconfigured_messages &= ~CONFIG_RATE_STATUS; + // position + _last_pos_time = _buffer.pvt.itow; + state.location.lng = _buffer.pvt.lon; + state.location.lat = _buffer.pvt.lat; + state.location.alt = _buffer.pvt.h_msl / 10; + switch (_buffer.pvt.fix_type) + { + case 0: + state.status = AP_GPS::NO_FIX; + break; + case 1: + state.status = AP_GPS::NO_FIX; + break; + case 2: + state.status = AP_GPS::GPS_OK_FIX_2D; + break; + case 3: + state.status = AP_GPS::GPS_OK_FIX_3D; + if (_buffer.pvt.flags & 0b00000010) // diffsoln + state.status = AP_GPS::GPS_OK_FIX_3D_DGPS; + if (_buffer.pvt.flags & 0b01000000) // carrsoln - float + state.status = AP_GPS::GPS_OK_FIX_3D_RTK; + if (_buffer.pvt.flags & 0b10000000) // carrsoln - fixed + state.status = AP_GPS::GPS_OK_FIX_3D_RTK; + break; + case 4: + state.status = AP_GPS::GPS_OK_FIX_3D; + break; + case 5: + state.status = AP_GPS::NO_FIX; + break; + default: + state.status = AP_GPS::NO_FIX; + break; + } + next_fix = state.status; + _new_position = true; + state.horizontal_accuracy = _buffer.pvt.h_acc*1.0e-3f; + state.vertical_accuracy = _buffer.pvt.v_acc*1.0e-3f; + state.have_horizontal_accuracy = true; + state.have_vertical_accuracy = true; + // SVs + state.num_sats = _buffer.pvt.num_sv; + // velocity + _last_vel_time = _buffer.pvt.itow; + state.ground_speed = _buffer.pvt.gspeed*0.001f; // m/s + state.ground_course = wrap_360(_buffer.pvt.head_mot * 1.0e-5f); // Heading 2D deg * 100000 + state.have_vertical_velocity = true; + state.velocity.x = _buffer.pvt.velN * 0.001f; + state.velocity.y = _buffer.pvt.velE * 0.001f; + state.velocity.z = _buffer.pvt.velD * 0.001f; + state.have_speed_accuracy = true; + state.speed_accuracy = _buffer.pvt.s_acc*0.001f; + _new_speed = true; + // dop + if(noReceivedHdop) { + state.hdop = _buffer.pvt.p_dop; + state.vdop = _buffer.pvt.p_dop; + } + + state.last_gps_time_ms = AP_HAL::millis(); + if (state.time_week_ms + 220 > _buffer.pvt.itow) { + // we got a 5Hz update. + _last_5hz_time = state.last_gps_time_ms; + } + + // time + state.time_week_ms = _buffer.pvt.itow; + break; case MSG_VELNED: Debug("MSG_VELNED"); + if (havePvtMsg) { + _configure_message_rate(_class, _msg_id, 0); + break; + } _last_vel_time = _buffer.velned.time; state.ground_speed = _buffer.velned.speed_2d*0.01f; // m/s state.ground_course = wrap_360(_buffer.velned.heading_2d * 1.0e-5f); // Heading 2D deg * 100000 diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 427513afd7..359cafea2a 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -53,6 +53,7 @@ #define RATE_POSLLH 1 #define RATE_STATUS 1 #define RATE_SOL 1 +#define RATE_PVT 1 #define RATE_VELNED 1 #define RATE_DOP 1 #define RATE_HW 5 @@ -71,10 +72,11 @@ #define CONFIG_NAV_SETTINGS (1<<10) #define CONFIG_GNSS (1<<11) #define CONFIG_SBAS (1<<12) +#define CONFIG_RATE_PVT (1<<13) #define CONFIG_ALL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_SOL | CONFIG_RATE_VELNED \ | CONFIG_RATE_DOP | CONFIG_RATE_MON_HW | CONFIG_RATE_MON_HW2 | CONFIG_RATE_RAW | CONFIG_VERSION \ - | CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS) + | CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS | CONFIG_RATE_PVT) //Configuration Sub-Sections #define SAVE_CFG_IO (1<<0) @@ -94,7 +96,7 @@ public: // Methods bool read(); - AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D_DGPS; } + AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D_RTK; } static bool _detect(struct UBLOX_detect_state &state, uint8_t data); @@ -227,6 +229,29 @@ private: uint8_t satellites; uint32_t res2; }; + struct PACKED ubx_nav_pvt { + uint32_t itow; + uint16_t year; + uint8_t month, day, hour, min, sec; + uint8_t valid; + uint32_t t_acc; + int32_t nano; + uint8_t fix_type; + uint8_t flags; + uint8_t flags2; + uint8_t num_sv; + int32_t lon, lat; + int32_t height, h_msl; + uint32_t h_acc, v_acc; + int32_t velN, velE, velD, gspeed; + int32_t head_mot; + uint32_t s_acc; + uint32_t head_acc; + uint16_t p_dop; + uint8_t reserved1[6]; + uint32_t headVeh; + uint8_t reserved2[4]; + }; struct PACKED ubx_nav_velned { uint32_t time; // GPS msToW int32_t ned_north; @@ -363,6 +388,7 @@ private: ubx_nav_status status; ubx_nav_dop dop; ubx_nav_solution solution; + ubx_nav_pvt pvt; ubx_nav_velned velned; ubx_cfg_msg_rate msg_rate; ubx_cfg_msg_rate_6 msg_rate_6; @@ -399,6 +425,7 @@ private: MSG_STATUS = 0x3, MSG_DOP = 0x4, MSG_SOL = 0x6, + MSG_PVT = 0x7, MSG_VELNED = 0x12, MSG_CFG_CFG = 0x09, MSG_CFG_RATE = 0x08, @@ -445,17 +472,18 @@ private: enum config_step { STEP_RATE_NAV = 0, - STEP_RATE_POSLLH, - STEP_RATE_VELNED, STEP_PORT, - STEP_POLL_SVINFO, - STEP_POLL_SBAS, - STEP_POLL_NAV, - STEP_POLL_GNSS, - STEP_NAV_RATE, + STEP_SOL, + STEP_PVT, + STEP_POLL_SVINFO, // poll svinfo + STEP_POLL_SBAS, // poll SBAS + STEP_POLL_NAV, // poll NAV settings + STEP_POLL_GNSS, // poll GNSS + STEP_NAV_RATE, // poll NAV rate + STEP_RATE_POSLLH, + STEP_RATE_VELNED, STEP_POSLLH, STEP_STATUS, - STEP_SOL, STEP_VELNED, STEP_DOP, STEP_MON_HW, @@ -508,6 +536,8 @@ private: bool _cfg_needs_save; bool noReceivedHdop; + + bool havePvtMsg; bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate); void _configure_rate(void); diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index d06842f912..a0fe1f2f16 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -248,11 +248,35 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d) uint16_t nDOP; uint16_t eDOP; } dop; + struct PACKED ubx_nav_pvt { + uint32_t itow; + uint16_t year; + uint8_t month, day, hour, min, sec; + uint8_t valid; + uint32_t t_acc; + int32_t nano; + uint8_t fix_type; + uint8_t flags; + uint8_t flags2; + uint8_t num_sv; + int32_t lon, lat; + int32_t height, h_msl; + uint32_t h_acc, v_acc; + int32_t velN, velE, velD, gspeed; + int32_t head_mot; + uint32_t s_acc; + uint32_t head_acc; + uint16_t p_dop; + uint8_t reserved1[6]; + uint32_t headVeh; + uint8_t reserved2[4]; + } pvt; const uint8_t MSG_POSLLH = 0x2; const uint8_t MSG_STATUS = 0x3; const uint8_t MSG_DOP = 0x4; const uint8_t MSG_VELNED = 0x12; const uint8_t MSG_SOL = 0x6; + const uint8_t MSG_PVT = 0x7; uint16_t time_week; uint32_t time_week_ms; @@ -302,12 +326,42 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d) dop.hDOP = 121; dop.nDOP = 65535; dop.eDOP = 65535; + + pvt.itow = time_week_ms; + pvt.year = 0; + pvt.month = 0; + pvt.day = 0; + pvt.hour = 0; + pvt.min = 0; + pvt.sec = 0; + pvt.valid = 0; // invalid utc date + pvt.t_acc = 0; + pvt.nano = 0; + pvt.fix_type = 0x3; + pvt.flags = 0b10000011; // carrsoln=fixed, psm = na, diffsoln and fixok + pvt.flags2 =0; + pvt.num_sv = d->have_lock?_sitl->gps_numsats:3; + pvt.lon = d->longitude * 1.0e7; + pvt.lat = d->latitude * 1.0e7; + pvt.height = d->altitude*1000.0f; + pvt.h_msl = d->altitude*1000.0f; + pvt.h_acc = 200; + pvt.v_acc = 200; + pvt.velN = 1000.0f * d->speedN; + pvt.velE = 1000.0f * d->speedE; + pvt.velD = 1000.0f * d->speedD; + pvt.gspeed = norm(d->speedN, d->speedE) * 1000; + pvt.head_mot = ToDeg(atan2f(d->speedE, d->speedN)) * 1.0e5; + pvt.s_acc = 40; + pvt.head_acc = 38 * 1.0e5; + pvt.p_dop = 65535; _gps_send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos)); _gps_send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status)); _gps_send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned)); _gps_send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol)); _gps_send_ubx(MSG_DOP, (uint8_t*)&dop, sizeof(dop)); + _gps_send_ubx(MSG_PVT, (uint8_t*)&pvt, sizeof(pvt)); } static void swap_uint32(uint32_t *v, uint8_t n)