Browse Source

Copter: position_ok true when EKF predicts it will be ok

This resolves the chicken and egg problem of the EKF filter status's
position flags not becoming true until after the vehicle has been armed
at least once.
mission-4.1.18
Randy Mackay 10 years ago
parent
commit
e464909ddf
  1. 6
      ArduCopter/system.pde

6
ArduCopter/system.pde

@ -340,7 +340,8 @@ static bool position_ok()
{ {
if (ahrs.have_inertial_nav()) { if (ahrs.have_inertial_nav()) {
// with EKF use filter status and ekf check // with EKF use filter status and ekf check
return (inertial_nav.get_filter_status().flags.horiz_pos_abs && !failsafe.ekf); nav_filter_status filt_status = inertial_nav.get_filter_status();
return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs) && !failsafe.ekf);
} else { } else {
// with interial nav use GPS based checks // with interial nav use GPS based checks
return (ap.home_is_set && gps.status() >= AP_GPS::GPS_OK_FIX_3D && return (ap.home_is_set && gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
@ -361,7 +362,8 @@ static bool optflow_position_ok()
} }
// get filter status from inertial nav or EKF // get filter status from inertial nav or EKF
return inertial_nav.get_filter_status().flags.horiz_pos_rel; nav_filter_status filt_status = inertial_nav.get_filter_status();
return (filt_status.flags.horiz_pos_rel || filt_status.flags.pred_horiz_pos_rel);
#endif #endif
} }

Loading…
Cancel
Save