AP_GPS: fixed constrained NaN in EKF3 caused by bad GPS blending

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 cba3d1358b
commit 773771dd8d

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
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;