aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: position_ok false when EKF in const pos mode

Browse Source
This commit is contained in:
Randy Mackay 2015-01-06 12:15:35 +09:00
parent 74ac79ba10
commit 01f1ce4cb3
1 changed files with 13 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/system.pde
View File

@ -339,9 +339,21 @@ static void startup_ground(bool force_gyro_cal)
static bool position_ok()
{
if (ahrs.have_inertial_nav()) {
// return false if ekf failsafe has triggered
if (failsafe.ekf) {
return false;
}
// with EKF use filter status and ekf check
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);
// if disarmed we accept a predicted horizontal position
if (!motors.armed()) {
return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs));
} else {
// once armed we require a good absolute position and EKF must not be in const_pos_mode
return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode);
}
} else {
// with interial nav use GPS based checks
return (ap.home_is_set && gps.status() >= AP_GPS::GPS_OK_FIX_3D &&