Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: fixed handling of loss of GPS yaw

Browse Source 
when the GPS yaw data doesn't pass the internal tests of validity for
400ms we need to stop telling the EKF that we have valid yaw
This commit is contained in:
Andrew Tridgell 2021-05-01 09:52:57 +10:00
parent e78c6804a0
commit 7cc11e6856
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -1351,7 +1351,6 @@ AP_GPS_UBLOX::_parse_gps(void)
MB_Debug("RELPOSNED ITOW %u %u\n", unsigned(_buffer.relposned.iTOW), unsigned(_last_relposned_itow));
}
_last_relposned_itow = _buffer.relposned.iTOW;
_last_relposned_ms = AP_HAL::millis();
if (((_buffer.relposned.flags & valid_mask) == valid_mask) &&
((_buffer.relposned.flags & invalid_mask) == 0) &&
@ -1360,6 +1359,7 @@ AP_GPS_UBLOX::_parse_gps(void)
_buffer.relposned.relPosD*0.01)) {
state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
state.have_gps_yaw_accuracy = true;
_last_relposned_ms = AP_HAL::millis();
} else {
state.have_gps_yaw_accuracy = false;
}
@ -1527,7 +1527,7 @@ AP_GPS_UBLOX::_parse_gps(void)
// PVT and the new RELPOSNED message so that we give a
// consistent view
if (AP_HAL::millis() - _last_relposned_ms > 400) {
// we have stopped receiving RELPOSNED messages, disable yaw reporting
// we have stopped receiving valid RELPOSNED messages, disable yaw reporting
state.have_gps_yaw = false;
} else if (_last_relposned_itow != _last_pvt_itow) {
// wait until ITOW matches