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 constrained NaN in EKF3 caused by bad GPS blending

Browse Source 
if the accuracies reported are very low then we can do a division by
zero and this results in a constraining NaN for GPS vertical velocity
filter in NavEKF3_core::calcGpsGoodToAlign
This commit is contained in:
Andrew Tridgell 2020-11-22 14:24:01 +11:00
parent 82ae3fe635
commit f8beca190c
1 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_GPS/AP_GPS.cpp
View File

@ -1541,6 +1541,10 @@ bool AP_GPS::calc_blend_weights(void)
} }
} }
if (!is_positive(sum_of_all_weights)) {
return false;
}
// calculate an overall weight // calculate an overall weight
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) { for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
_blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights; _blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights;