Browse Source

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
c415-sdk
Andrew Tridgell 4 years ago
parent
commit
a762926526
  1. 4
      libraries/AP_GPS/AP_GPS.cpp

4
libraries/AP_GPS/AP_GPS.cpp

@ -1623,6 +1623,10 @@ bool AP_GPS::calc_blend_weights(void) @@ -1623,6 +1623,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…
Cancel
Save