mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
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:
parent
82ae3fe635
commit
f8beca190c
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user