mirror of https://github.com/ArduPilot/ardupilot
Copter: ekf failsafe disabled for acro and stabilize
Also restructure to combine ekf_over_threshold and ekf_check_position_problem
This commit is contained in:
parent
f8464577ac
commit
656892cd88
|
@ -718,7 +718,6 @@ private:
|
||||||
|
|
||||||
// ekf_check.cpp
|
// ekf_check.cpp
|
||||||
void ekf_check();
|
void ekf_check();
|
||||||
bool ekf_check_position_problem();
|
|
||||||
bool ekf_over_threshold();
|
bool ekf_over_threshold();
|
||||||
void failsafe_ekf_event();
|
void failsafe_ekf_event();
|
||||||
void failsafe_ekf_off_event(void);
|
void failsafe_ekf_off_event(void);
|
||||||
|
|
|
@ -44,7 +44,7 @@ void Copter::ekf_check()
|
||||||
}
|
}
|
||||||
|
|
||||||
// compare compass and velocity variance vs threshold
|
// compare compass and velocity variance vs threshold
|
||||||
if (ekf_over_threshold() || ekf_check_position_problem()) {
|
if (ekf_over_threshold()) {
|
||||||
// if compass is not yet flagged as bad
|
// if compass is not yet flagged as bad
|
||||||
if (!ekf_check_state.bad_variance) {
|
if (!ekf_check_state.bad_variance) {
|
||||||
// increase counter
|
// increase counter
|
||||||
|
@ -86,38 +86,6 @@ void Copter::ekf_check()
|
||||||
// To-Do: add ekf variances to extended status
|
// To-Do: add ekf variances to extended status
|
||||||
}
|
}
|
||||||
|
|
||||||
// ekf_check_position_problem - returns true if the EKF has a positioning problem
|
|
||||||
bool Copter::ekf_check_position_problem()
|
|
||||||
{
|
|
||||||
// either otflow or abs means we're OK:
|
|
||||||
if (optflow_position_ok()) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
if (ekf_position_ok()) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// We don't know where we are. Is this a problem?
|
|
||||||
if (copter.flightmode->requires_GPS()) {
|
|
||||||
// Oh, yes, we have a problem
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
// sometimes LAND *does* require GPS:
|
|
||||||
if (control_mode == LAND && landing_with_GPS()) {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
// we're in a non-GPS mode (e.g. althold/stabilize)
|
|
||||||
|
|
||||||
if (g.fs_ekf_action == FS_EKF_ACTION_LAND_EVEN_STABILIZE) {
|
|
||||||
// the user is making an issue out of it
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
|
// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
|
||||||
bool Copter::ekf_over_threshold()
|
bool Copter::ekf_over_threshold()
|
||||||
{
|
{
|
||||||
|
@ -144,7 +112,15 @@ bool Copter::ekf_over_threshold()
|
||||||
over_thresh_count++;
|
over_thresh_count++;
|
||||||
}
|
}
|
||||||
|
|
||||||
return (over_thresh_count >= 2);
|
if (over_thresh_count >= 2) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// either optflow relative or absolute position estimate OK
|
||||||
|
if (optflow_position_ok() || ekf_position_ok()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -160,6 +136,17 @@ void Copter::failsafe_ekf_event()
|
||||||
failsafe.ekf = true;
|
failsafe.ekf = true;
|
||||||
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED);
|
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED);
|
||||||
|
|
||||||
|
// does this mode require position?
|
||||||
|
if (!copter.flightmode->requires_GPS() && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// sometimes LAND *does* require GPS:
|
||||||
|
if (control_mode == LAND && landing_with_GPS()) {
|
||||||
|
mode_land.do_not_use_GPS();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
// take action based on fs_ekf_action parameter
|
// take action based on fs_ekf_action parameter
|
||||||
switch (g.fs_ekf_action) {
|
switch (g.fs_ekf_action) {
|
||||||
case FS_EKF_ACTION_ALTHOLD:
|
case FS_EKF_ACTION_ALTHOLD:
|
||||||
|
@ -168,15 +155,12 @@ void Copter::failsafe_ekf_event()
|
||||||
set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
|
set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
case FS_EKF_ACTION_LAND:
|
||||||
|
case FS_EKF_ACTION_LAND_EVEN_STABILIZE:
|
||||||
default:
|
default:
|
||||||
set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
|
set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
// if flight mode is already LAND ensure it's not the GPS controlled LAND
|
|
||||||
if (control_mode == LAND) {
|
|
||||||
mode_land.do_not_use_GPS();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
|
// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
|
||||||
|
|
Loading…
Reference in New Issue