aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: ensure that PVT and RELPOSNED msgs time align

Browse Source 
this ensures we wait until we have a matching PVT and RELPOSNED
message before we report new data. If we stop receiving RELPOSNED
messages then disable yaw support
This commit is contained in:
Andrew Tridgell 2020-04-03 11:51:22 +11:00
parent 3fbeae613b
commit 9a6de209ab
2 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -1333,6 +1333,10 @@ AP_GPS_UBLOX::_parse_gps(void)
const float rel_dist = _buffer.relposned.relPosLength * 1.0e-2;
const float strict_length_error_allowed = 0.2; // allow for up to 20% error
const float min_separation = 0.05;
_check_new_itow(_buffer.relposned.iTOW);
_last_relposned_itow = _buffer.relposned.iTOW;
_last_relposned_ms = AP_HAL::millis();
/*
RELPOSNED messages gives the NED distance from base to
rover. It comes from the rover
@ -1366,6 +1370,7 @@ AP_GPS_UBLOX::_parse_gps(void)
havePvtMsg = true;
// position
_check_new_itow(_buffer.pvt.itow);
_last_pvt_itow = _buffer.pvt.itow;
_last_pos_time = _buffer.pvt.itow;
state.location.lng = _buffer.pvt.lon;
state.location.lat = _buffer.pvt.lat;
@ -1514,6 +1519,20 @@ AP_GPS_UBLOX::_parse_gps(void)
return false;
}
if (role == AP_GPS::GPS_ROLE_MB_ROVER) {
// when we are a rover we want to ensure we have both the new
// PVT and the new RELPOSNED message so that we give a
// consistent view
if (state.have_gps_yaw && AP_HAL::millis() - _last_relposned_ms > 400) {
// we have stopped receiving RELPOSNED messages, disable yaw reporting
state.have_gps_yaw = false;
}
if (state.have_gps_yaw && _last_relposned_itow != _last_pvt_itow) {
// wait until ITOW matches
return false;
}
}
// we only return true when we get new position and speed data
// this ensures we don't use stale data
if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {

3
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -681,6 +681,9 @@ private:
struct ubx_mon_ver _version;
uint32_t _unconfigured_messages;
uint8_t _hardware_generation;
uint32_t _last_pvt_itow;
uint32_t _last_relposned_itow;
uint32_t _last_relposned_ms;
// the role set from GPS_TYPE
AP_GPS::GPS_Role role;