From 8ecac27777f59d9aacfbb032b171e4566d3dbd73 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 21 Jan 2021 12:42:47 +0900 Subject: [PATCH] AP_NavEKF_Source; pre_arm_check may skip position checks --- libraries/AP_NavEKF/AP_NavEKF_Source.cpp | 169 ++++++++++++----------- libraries/AP_NavEKF/AP_NavEKF_Source.h | 3 +- 2 files changed, 88 insertions(+), 84 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp index 909ee63c89..b47806df74 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp @@ -299,7 +299,8 @@ void AP_NavEKF_Source::mark_configured_in_storage() } // returns false if we fail arming checks, in which case the buffer will be populated with a failure message -bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const +// requires_position should be true if horizontal position configuration should be checked +bool AP_NavEKF_Source::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const { auto &dal = AP::dal(); bool baro_required = false; @@ -314,91 +315,93 @@ bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) // check source params are valid for (uint8_t i=0; isnprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSXY", (int)i+1); - return false; - } + if (requires_position) { + // check posxy + switch ((SourceXY)_source_set[i].posxy.get()) { + case SourceXY::NONE: + break; + case SourceXY::GPS: + gps_required = true; + break; + case SourceXY::BEACON: + beacon_required = true; + break; + case SourceXY::EXTNAV: + visualodom_required = true; + break; + case SourceXY::OPTFLOW: + case SourceXY::WHEEL_ENCODER: + default: + // invalid posxy value + hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSXY", (int)i+1); + return false; + } - // check velxy - switch ((SourceXY)_source_set[i].velxy.get()) { - case SourceXY::NONE: - break; - case SourceXY::GPS: - gps_required = true; - break; - case SourceXY::OPTFLOW: - optflow_required = true; - break; - case SourceXY::EXTNAV: - visualodom_required = true; - break; - case SourceXY::WHEEL_ENCODER: - wheelencoder_required = true; - break; - case SourceXY::BEACON: - default: - // invalid velxy value - hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELXY", (int)i+1); - return false; - } + // check velxy + switch ((SourceXY)_source_set[i].velxy.get()) { + case SourceXY::NONE: + break; + case SourceXY::GPS: + gps_required = true; + break; + case SourceXY::OPTFLOW: + optflow_required = true; + break; + case SourceXY::EXTNAV: + visualodom_required = true; + break; + case SourceXY::WHEEL_ENCODER: + wheelencoder_required = true; + break; + case SourceXY::BEACON: + default: + // invalid velxy value + hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELXY", (int)i+1); + return false; + } - // check posz - switch ((SourceZ)_source_set[i].posz.get()) { - case SourceZ::BARO: - baro_required = true; - break; - case SourceZ::RANGEFINDER: - rangefinder_required = true; - break; - case SourceZ::GPS: - gps_required = true; - break; - case SourceZ::BEACON: - beacon_required = true; - break; - case SourceZ::EXTNAV: - visualodom_required = true; - break; - case SourceZ::NONE: - default: - // invalid posz value - hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSZ", (int)i+1); - return false; - } + // check posz + switch ((SourceZ)_source_set[i].posz.get()) { + case SourceZ::BARO: + baro_required = true; + break; + case SourceZ::RANGEFINDER: + rangefinder_required = true; + break; + case SourceZ::GPS: + gps_required = true; + break; + case SourceZ::BEACON: + beacon_required = true; + break; + case SourceZ::EXTNAV: + visualodom_required = true; + break; + case SourceZ::NONE: + default: + // invalid posz value + hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSZ", (int)i+1); + return false; + } - // check velz - switch ((SourceZ)_source_set[i].velz.get()) { - case SourceZ::NONE: - break; - case SourceZ::GPS: - gps_required = true; - break; - case SourceZ::EXTNAV: - visualodom_required = true; - break; - case SourceZ::BARO: - case SourceZ::RANGEFINDER: - case SourceZ::BEACON: - default: - // invalid velz value - hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELZ", (int)i+1); - return false; + // check velz + switch ((SourceZ)_source_set[i].velz.get()) { + case SourceZ::NONE: + break; + case SourceZ::GPS: + gps_required = true; + break; + case SourceZ::EXTNAV: + visualodom_required = true; + break; + case SourceZ::BARO: + case SourceZ::RANGEFINDER: + case SourceZ::BEACON: + default: + // invalid velz value + hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELZ", (int)i+1); + return false; + } } // check yaw diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.h b/libraries/AP_NavEKF/AP_NavEKF_Source.h index dc3a1b7d94..5b5c576aa4 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_Source.h +++ b/libraries/AP_NavEKF/AP_NavEKF_Source.h @@ -90,7 +90,8 @@ public: void mark_configured_in_storage(); // returns false if we fail arming checks, in which case the buffer will be populated with a failure message - bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const; + // requires_position should be true if horizontal position configuration should be checked + bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const; // return true if ext nav is enabled on any source bool ext_nav_enabled(void) const;