Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: position_ok true when EKF predicts it will be ok

Browse Source 
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.
This commit is contained in:
Randy Mackay 2015-01-05 14:14:27 +09:00
parent 58ac9de94b
commit e464909ddf
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/system.pde
View File

@ -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
} }