From 67d8965cade467a0cf06a4c974359e72217418ae Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 3 Aug 2022 22:59:38 +1000 Subject: [PATCH] ArduCopter: make terrain checks common between Copter and Plane --- ArduCopter/AP_Arming.cpp | 28 ++++++++++++---------------- ArduCopter/AP_Arming.h | 4 ++++ 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 18cdceeca0..2f1f3a0756 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -120,6 +120,17 @@ bool AP_Arming_Copter::board_voltage_checks(bool display_failure) return true; } +// expected to return true if the terrain database is required to have +// all data loaded +bool AP_Arming_Copter::terrain_database_required() const +{ + if (copter.wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE && + copter.mode_rtl.get_alt_type() == ModeRTL::RTLAltType::RTL_ALTTYPE_TERRAIN) { + return true; + } + return AP_Arming::terrain_database_required(); +} + bool AP_Arming_Copter::parameter_checks(bool display_failure) { // check various parameter values @@ -228,22 +239,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) } break; case AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE: -#if AP_TERRAIN_AVAILABLE - if (!copter.terrain.enabled()) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "terrain disabled"); - return false; - } - // check terrain data is loaded - uint16_t terr_pending, terr_loaded; - copter.terrain.get_statistics(terr_pending, terr_loaded); - if (terr_pending != 0) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "waiting for terrain data"); - return false; - } -#else - check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "terrain disabled"); - return false; -#endif + // these checks are done in AP_Arming break; } } diff --git a/ArduCopter/AP_Arming.h b/ArduCopter/AP_Arming.h index e1a32ed20a..58f8e78670 100644 --- a/ArduCopter/AP_Arming.h +++ b/ArduCopter/AP_Arming.h @@ -51,6 +51,10 @@ protected: void set_pre_arm_check(bool b); + // expected to return true if the terrain database is required to have + // all data loaded + bool terrain_database_required() const override; + private: // actually contains the pre-arm checks. This is wrapped so that