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

Copter: use static assert to simplify use of EKF_CHECK_ITERATIONS_MAX

Browse Source
This commit is contained in:
Randy Mackay 2020-04-23 11:04:38 +09:00 committed by Andrew Tridgell
parent 0fb233ed4b
commit ddaa5dee86
1 changed files with 6 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduCopter/ekf_check.cpp
View File

@ -28,6 +28,9 @@ static struct {
// should be called at 10hz // should be called at 10hz
void Copter::ekf_check() void Copter::ekf_check()
{ {
// ensure EKF_CHECK_ITERATIONS_MAX is at least 7
static_assert(EKF_CHECK_ITERATIONS_MAX >= 7, "EKF_CHECK_ITERATIONS_MAX must be at least 7");
// exit immediately if ekf has no origin yet - this assumes the origin can never become unset // exit immediately if ekf has no origin yet - this assumes the origin can never become unset
Location temp_loc; Location temp_loc;
if (!ahrs.get_origin(temp_loc)) { if (!ahrs.get_origin(temp_loc)) {
@ -49,20 +52,16 @@ void Copter::ekf_check()
if (!ekf_check_state.bad_variance) { if (!ekf_check_state.bad_variance) {
// increase counter // increase counter
ekf_check_state.fail_count++; ekf_check_state.fail_count++;
#if EKF_CHECK_ITERATIONS_MAX > 3 if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-2)) {
if (ekf_check_state.fail_count == MIN((EKF_CHECK_ITERATIONS_MAX-2), 5)) { // we are two iterations away from declaring an EKF failsafe, ask the EKF if we can reset
// we are just about to declare a EKF failsafe, ask the EKF if we can reset
// yaw to resolve the issue // yaw to resolve the issue
ahrs.request_yaw_reset(); ahrs.request_yaw_reset();
} }
#endif if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-1)) {
#if EKF_CHECK_ITERATIONS_MAX > 2
if (ekf_check_state.fail_count == EKF_CHECK_ITERATIONS_MAX-1) {
// we are just about to declare a EKF failsafe, ask the EKF if we can // we are just about to declare a EKF failsafe, ask the EKF if we can
// change lanes to resolve the issue // change lanes to resolve the issue
ahrs.check_lane_switch(); ahrs.check_lane_switch();
} }
#endif
// if counter above max then trigger failsafe // if counter above max then trigger failsafe
if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) { if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
// limit count from climbing too high // limit count from climbing too high