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

Copter: factor out a ekf_check_position_problem method

Browse Source
This commit is contained in:
Peter Barker 2017-09-15 11:54:03 +10:00 committed by Francisco Ferreira
parent 84a873a520
commit ff82f23cf6
2 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -972,6 +972,7 @@ private:
void ekf_check();
bool ekf_over_threshold();
bool ekf_check_position_problem();
void failsafe_ekf_event();
void failsafe_ekf_off_event(void);
void esc_calibration_startup_check();

22
ArduCopter/ekf_check.cpp
View File

@ -44,7 +44,7 @@ void Copter::ekf_check()
}
// compare compass and velocity variance vs threshold
if (ekf_over_threshold()) {
if (ekf_over_threshold() || ekf_check_position_problem()) {
// if compass is not yet flagged as bad
if (!ekf_check_state.bad_variance) {
// increase counter
@ -86,6 +86,21 @@ void Copter::ekf_check()
// 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;
}
return true;
}
// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
bool Copter::ekf_over_threshold()
{
@ -94,11 +109,6 @@ bool Copter::ekf_over_threshold()
return false;
}
// return true immediately if position is bad
if (!ekf_position_ok() && !optflow_position_ok()) {
return true;
}
// use EKF to get variance
float position_variance, vel_variance, height_variance, tas_variance;
Vector3f mag_variance;