From 5f9abc0406f3d5cbf17a2bf53c3af24bde024d3f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 17 Jul 2024 10:33:25 +1000 Subject: [PATCH] Copter: add and use private ModeAuto::option_is_enabled method --- ArduCopter/mode.h | 3 ++- ArduCopter/mode_auto.cpp | 18 +++++++++++++----- 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index c064900808..721cef3f0a 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -596,12 +596,13 @@ protected: private: - enum class Options : int32_t { + enum class Option : int32_t { AllowArming = (1 << 0U), AllowTakeOffWithoutRaisingThrottle = (1 << 1U), IgnorePilotYaw = (1 << 2U), AllowWeatherVaning = (1 << 7U), }; + bool option_is_enabled(Option option) const; // Enter auto rtl pseudo mode bool enter_auto_rtl(ModeReason reason); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index b5e9a62eec..3ac943f06d 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -202,15 +202,23 @@ void ModeAuto::set_submode(SubMode new_submode) } } +bool ModeAuto::option_is_enabled(Option option) const +{ + return ((copter.g2.auto_options & (uint32_t)option) != 0); +} + bool ModeAuto::allows_arming(AP_Arming::Method method) const { - return ((copter.g2.auto_options & (uint32_t)Options::AllowArming) != 0) && !auto_RTL; -}; + if (auto_RTL) { + return false; + } + return option_is_enabled(Option::AllowArming); +} #if WEATHERVANE_ENABLED == ENABLED bool ModeAuto::allows_weathervaning() const { - return (copter.g2.auto_options & (uint32_t)Options::AllowWeatherVaning) != 0; + return option_is_enabled(Option::AllowWeatherVaning); } #endif @@ -638,7 +646,7 @@ void PayloadPlace::start_descent() // returns true if pilot's yaw input should be used to adjust vehicle's heading bool ModeAuto::use_pilot_yaw(void) const { - const bool allow_yaw_option = (copter.g2.auto_options.get() & uint32_t(Options::IgnorePilotYaw)) == 0; + const bool allow_yaw_option = !option_is_enabled(Option::IgnorePilotYaw); const bool rtl_allow_yaw = (_mode == SubMode::RTL) && copter.mode_rtl.use_pilot_yaw(); const bool landing = _mode == SubMode::LAND; return allow_yaw_option || rtl_allow_yaw || landing; @@ -1039,7 +1047,7 @@ void ModeAuto::takeoff_run() { // if the user doesn't want to raise the throttle we can set it automatically // note that this can defeat the disarm check on takeoff - if ((copter.g2.auto_options & (int32_t)Options::AllowTakeOffWithoutRaisingThrottle) != 0) { + if (option_is_enabled(Option::AllowTakeOffWithoutRaisingThrottle)) { copter.set_auto_armed(true); } auto_takeoff.run();