Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: fix ekf-check reliance on position_ok

This commit is contained in:
Randy Mackay 2015-06-15 15:53:41 +09:00
parent 46926765c4
commit f603c1ef69
3 changed files with 13 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -876,6 +876,7 @@ private:
void init_ardupilot(); void init_ardupilot();
void startup_ground(bool force_gyro_cal); void startup_ground(bool force_gyro_cal);
bool position_ok(); bool position_ok();
bool ekf_position_ok();
bool optflow_position_ok(); bool optflow_position_ok();
void update_auto_armed(); void update_auto_armed();
void check_usb_mux(void); void check_usb_mux(void);

2
ArduCopter/ekf_check.cpp
View File

@ -91,7 +91,7 @@ bool Copter::ekf_over_threshold()
} }
// return true immediately if position is bad // return true immediately if position is bad
if (!position_ok() && !optflow_position_ok()) { if (!ekf_position_ok() && !optflow_position_ok()) {
return true; return true;
} }

15
ArduCopter/system.cpp
View File

@ -309,13 +309,20 @@ void Copter::startup_ground(bool force_gyro_cal)
// position_ok - returns true if the horizontal absolute position is ok and home position is set // position_ok - returns true if the horizontal absolute position is ok and home position is set
bool Copter::position_ok() bool Copter::position_ok()
{ {
if (!ahrs.have_inertial_nav()) { // return false if ekf failsafe has triggered
// do not allow navigation with dcm position if (failsafe.ekf) {
return false; return false;
} }
// return false if ekf failsafe has triggered // check ekf position estimate
if (failsafe.ekf) { return ekf_position_ok();
}
// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set
bool Copter::ekf_position_ok()
{
if (!ahrs.have_inertial_nav()) {
// do not allow navigation with dcm position
return false; return false;
} }