Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: prevent switching to a dead GPS 

when we switch away from the blended GPS instance we need to ensure we
don't switch to a GPS that is timing out, and may be the instance that
is triggering the disable of blending
This commit is contained in:
Andrew Tridgell 2022-02-01 20:34:22 +11:00
parent 699651e306
commit feb9e78f00
1 changed files with 25 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_GPS/AP_GPS.cpp
View File

@ -1124,13 +1124,34 @@ void AP_GPS::update_primary(void)
if (primary_instance == GPS_BLENDED_INSTANCE) {
primary_instance = 0;
for (uint8_t i=1; i<GPS_MAX_RECEIVERS; i++) {
// choose GPS with highest state or higher number of satellites
if ((state[i].status > state[primary_instance].status) ||
((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats))) {
// choose GPS with highest state or higher number of
// satellites. Reject a GPS with an old update time, as it
// may be the old timestamp that triggered the loss of
// blending
const uint32_t delay_threshold = 400;
const bool higher_status = state[i].status > state[primary_instance].status;
const bool old_data_primary = (now - state[primary_instance].last_gps_time_ms) > delay_threshold;
const bool old_data = (now - state[i].last_gps_time_ms) > delay_threshold;
const bool equal_status = state[i].status == state[primary_instance].status;
const bool more_sats = state[i].num_sats > state[primary_instance].num_sats;
if (old_data && !old_data_primary) {
// don't switch to a GPS that has not updated in 400ms
continue;
}
if (state[i].status < GPS_OK_FIX_3D) {
// don't use a GPS without 3D fix
continue;
}
// select the new GPS if the primary has old data, or new
// GPS either has higher status, or has the same status
// and more satellites
if ((old_data_primary && !old_data) ||
higher_status ||
(equal_status && more_sats)) {
primary_instance = i;
_last_instance_swap_ms = now;
}
}
_last_instance_swap_ms = now;
return;
}
#endif // defined(GPS_BLENDED_INSTANCE)