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

ArduPlane: Allow EKF dead reckoning to continue after loss of GPS

Browse Source
This commit is contained in:
Paul Riseborough 2020-05-09 22:32:05 +10:00 committed by Andrew Tridgell
parent 40cc5a5006
commit cd5714d059
2 changed files with 1 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/Plane.h
View File

@ -1042,7 +1042,6 @@ private:
bool ekf_over_threshold();
void failsafe_ekf_event();
void failsafe_ekf_off_event(void);
bool ekf_position_ok() const;
public:
void failsafe_check(void);

28
ArduPlane/ekf_check.cpp
View File

@ -134,10 +134,7 @@ bool Plane::ekf_over_threshold()
return true;
}
if (ekf_position_ok()) {
return false;
}
return true;
return false;
}
@ -177,27 +174,4 @@ void Plane::failsafe_ekf_off_event(void)
ekf_check_state.failsafe_on = false;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED);
}
// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set
bool Plane::ekf_position_ok() const
{
if (!ahrs.have_inertial_nav()) {
// do not allow navigation with dcm position
return false;
}
// with EKF use filter status and ekf check
nav_filter_status filt_status;
if(ahrs.get_filter_status(filt_status)) {
}
// if disarmed we accept a predicted horizontal position
if (!plane.arming.is_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);
}
}