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

AP_GPS: don't accept infinite accuracies for blending 

these result in NaN values for velocities
This commit is contained in:
Andrew Tridgell 2020-11-23 09:33:20 +11:00
parent d359cdff3f
commit 0dafabf552
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_GPS/AP_GPS.cpp
View File

@ -1415,6 +1415,11 @@ bool AP_GPS::calc_blend_weights(void)
if (get_rate_ms(i) > max_rate_ms) { if (get_rate_ms(i) > max_rate_ms) {
max_rate_ms = get_rate_ms(i); max_rate_ms = get_rate_ms(i);
} }
if (isinf(state[i].speed_accuracy) ||
isinf(state[i].horizontal_accuracy) ||
isinf(state[i].vertical_accuracy)) {
return false;
}
} }
if ((int32_t)(max_ms - min_ms) < (int32_t)(2 * max_rate_ms)) { if ((int32_t)(max_ms - min_ms) < (int32_t)(2 * max_rate_ms)) {
// data is not too delayed so use the oldest time_stamp to give a chance for data from that receiver to be updated // data is not too delayed so use the oldest time_stamp to give a chance for data from that receiver to be updated