From a1ea1306a159f340dbc37890eb98e3afa4a24980 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 23 Dec 2020 11:47:58 +1100 Subject: [PATCH] Plane: use pure-virtual method for allows_throttle_nudging In place of a state variable which could become stale --- ArduPlane/Plane.h | 3 --- ArduPlane/mode.h | 17 +++++++++++++++++ ArduPlane/mode_acro.cpp | 1 - ArduPlane/mode_auto.cpp | 1 - ArduPlane/mode_autotune.cpp | 1 - ArduPlane/mode_circle.cpp | 1 - ArduPlane/mode_cruise.cpp | 1 - ArduPlane/mode_fbwa.cpp | 1 - ArduPlane/mode_fbwb.cpp | 1 - ArduPlane/mode_guided.cpp | 1 - ArduPlane/mode_initializing.cpp | 1 - ArduPlane/mode_loiter.cpp | 1 - ArduPlane/mode_manual.cpp | 1 - ArduPlane/mode_qacro.cpp | 1 - ArduPlane/mode_qstabilize.cpp | 1 - ArduPlane/mode_rtl.cpp | 1 - ArduPlane/mode_stabilize.cpp | 1 - ArduPlane/mode_takeoff.cpp | 1 - ArduPlane/mode_thermal.cpp | 1 - ArduPlane/mode_training.cpp | 1 - ArduPlane/navigation.cpp | 2 +- 21 files changed, 18 insertions(+), 22 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index f422d3c315..8d4388e971 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -579,9 +579,6 @@ private: // true if we are in an auto-navigation mode, which controls whether control input is ignored // with STICK_MIXING=0 bool auto_navigation_mode:1; - - // this allows certain flight modes to mix RC input with throttle depending on airspeed_nudge_cm - bool throttle_allows_nudging:1; // this controls throttle suppression in auto modes bool throttle_suppressed; diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 4b07aa4dd2..c0f07e594c 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -83,6 +83,10 @@ public: // subclasses override this if they require navigation. virtual void navigate() { return; } + // this allows certain flight modes to mix RC input with throttle + // depending on airspeed_nudge_cm + virtual bool allows_throttle_nudging() const { return false; } + protected: // subclasses override this to perform checks before entering the mode @@ -122,6 +126,8 @@ public: void navigate() override; + bool allows_throttle_nudging() const override { return true; } + protected: bool _enter() override; @@ -161,6 +167,8 @@ public: virtual bool is_guided_mode() const override { return true; } + bool allows_throttle_nudging() const override { return true; } + protected: bool _enter() override; @@ -234,6 +242,8 @@ public: void navigate() override; + bool allows_throttle_nudging() const override { return true; } + protected: bool _enter() override; @@ -282,6 +292,8 @@ public: // methods that affect movement of the vehicle in this mode void update() override { } + bool allows_throttle_nudging() const override { return true; } + protected: bool _enter() override; @@ -381,6 +393,7 @@ public: bool is_vtol_mode() const override { return true; } bool is_vtol_man_throttle() const override { return true; } virtual bool is_vtol_man_mode() const override { return true; } + bool allows_throttle_nudging() const override { return true; } // methods that affect movement of the vehicle in this mode void update() override; @@ -521,6 +534,8 @@ public: void navigate() override; + bool allows_throttle_nudging() const override { return true; } + // var_info for holding parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -554,6 +569,8 @@ public: void navigate() override; + bool allows_throttle_nudging() const override { return true; } + protected: bool exit_heading_aligned() const; diff --git a/ArduPlane/mode_acro.cpp b/ArduPlane/mode_acro.cpp index 8b13df6589..31449e3d81 100644 --- a/ArduPlane/mode_acro.cpp +++ b/ArduPlane/mode_acro.cpp @@ -3,7 +3,6 @@ bool ModeAcro::_enter() { - plane.throttle_allows_nudging = false; plane.auto_throttle_mode = false; plane.auto_navigation_mode = false; plane.acro_state.locked_roll = false; diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 7459dd7dfd..467929d0aa 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -3,7 +3,6 @@ bool ModeAuto::_enter() { - plane.throttle_allows_nudging = true; plane.auto_throttle_mode = true; plane.auto_navigation_mode = true; if (plane.quadplane.available() && plane.quadplane.enable == 2) { diff --git a/ArduPlane/mode_autotune.cpp b/ArduPlane/mode_autotune.cpp index 35efa4bf02..e4b8b120f3 100644 --- a/ArduPlane/mode_autotune.cpp +++ b/ArduPlane/mode_autotune.cpp @@ -3,7 +3,6 @@ bool ModeAutoTune::_enter() { - plane.throttle_allows_nudging = false; plane.auto_throttle_mode = false; plane.auto_navigation_mode = false; plane.autotune_start(); diff --git a/ArduPlane/mode_circle.cpp b/ArduPlane/mode_circle.cpp index 413efea01a..2d51839a26 100644 --- a/ArduPlane/mode_circle.cpp +++ b/ArduPlane/mode_circle.cpp @@ -4,7 +4,6 @@ bool ModeCircle::_enter() { // the altitude to circle at is taken from the current altitude - plane.throttle_allows_nudging = false; plane.auto_throttle_mode = true; plane.auto_navigation_mode = true; plane.next_WP_loc.alt = plane.current_loc.alt; diff --git a/ArduPlane/mode_cruise.cpp b/ArduPlane/mode_cruise.cpp index d0703df46b..a1c2d88b4c 100644 --- a/ArduPlane/mode_cruise.cpp +++ b/ArduPlane/mode_cruise.cpp @@ -3,7 +3,6 @@ bool ModeCruise::_enter() { - plane.throttle_allows_nudging = false; plane.auto_throttle_mode = true; plane.auto_navigation_mode = false; locked_heading = false; diff --git a/ArduPlane/mode_fbwa.cpp b/ArduPlane/mode_fbwa.cpp index 0f165010d1..3c610e6c7d 100644 --- a/ArduPlane/mode_fbwa.cpp +++ b/ArduPlane/mode_fbwa.cpp @@ -3,7 +3,6 @@ bool ModeFBWA::_enter() { - plane.throttle_allows_nudging = false; plane.auto_throttle_mode = false; plane.auto_navigation_mode = false; diff --git a/ArduPlane/mode_fbwb.cpp b/ArduPlane/mode_fbwb.cpp index df7c1ef334..280ea26108 100644 --- a/ArduPlane/mode_fbwb.cpp +++ b/ArduPlane/mode_fbwb.cpp @@ -3,7 +3,6 @@ bool ModeFBWB::_enter() { - plane.throttle_allows_nudging = false; plane.auto_throttle_mode = true; plane.auto_navigation_mode = false; diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index d1bc47cc27..7210e90001 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -3,7 +3,6 @@ bool ModeGuided::_enter() { - plane.throttle_allows_nudging = true; plane.auto_throttle_mode = true; plane.auto_navigation_mode = true; plane.guided_throttle_passthru = false; diff --git a/ArduPlane/mode_initializing.cpp b/ArduPlane/mode_initializing.cpp index 7aa10b8dbe..c7f5cc2ed1 100644 --- a/ArduPlane/mode_initializing.cpp +++ b/ArduPlane/mode_initializing.cpp @@ -3,7 +3,6 @@ bool ModeInitializing::_enter() { - plane.throttle_allows_nudging = true; plane.auto_throttle_mode = true; plane.auto_navigation_mode = false; diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index e2d4d1d736..c6ee086394 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -3,7 +3,6 @@ bool ModeLoiter::_enter() { - plane.throttle_allows_nudging = true; plane.auto_throttle_mode = true; plane.auto_navigation_mode = true; plane.do_loiter_at_location(); diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp index 1b27a79e78..1030d53e34 100644 --- a/ArduPlane/mode_manual.cpp +++ b/ArduPlane/mode_manual.cpp @@ -3,7 +3,6 @@ bool ModeManual::_enter() { - plane.throttle_allows_nudging = false; plane.auto_throttle_mode = false; plane.auto_navigation_mode = false; diff --git a/ArduPlane/mode_qacro.cpp b/ArduPlane/mode_qacro.cpp index 370d458a68..b2c25d6507 100644 --- a/ArduPlane/mode_qacro.cpp +++ b/ArduPlane/mode_qacro.cpp @@ -3,7 +3,6 @@ bool ModeQAcro::_enter() { - plane.throttle_allows_nudging = false; plane.auto_navigation_mode = false; if (!plane.quadplane.init_mode() && plane.previous_mode != nullptr) { plane.control_mode = plane.previous_mode; diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index 089769974a..4349764a46 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/ArduPlane/mode_qstabilize.cpp @@ -3,7 +3,6 @@ bool ModeQStabilize::_enter() { - plane.throttle_allows_nudging = true; plane.auto_navigation_mode = false; if (!plane.quadplane.init_mode() && plane.previous_mode != nullptr) { plane.control_mode = plane.previous_mode; diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index bd581a5793..00e4474d28 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -3,7 +3,6 @@ bool ModeRTL::_enter() { - plane.throttle_allows_nudging = true; plane.auto_throttle_mode = true; plane.auto_navigation_mode = true; plane.prev_WP_loc = plane.current_loc; diff --git a/ArduPlane/mode_stabilize.cpp b/ArduPlane/mode_stabilize.cpp index 27ecff0d2e..23737b5b65 100644 --- a/ArduPlane/mode_stabilize.cpp +++ b/ArduPlane/mode_stabilize.cpp @@ -3,7 +3,6 @@ bool ModeStabilize::_enter() { - plane.throttle_allows_nudging = false; plane.auto_throttle_mode = false; plane.auto_navigation_mode = false; diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index ef6d1228be..cd7d8a8fc9 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -52,7 +52,6 @@ ModeTakeoff::ModeTakeoff() bool ModeTakeoff::_enter() { // the altitude to circle at is taken from the current altitude - plane.throttle_allows_nudging = true; plane.auto_throttle_mode = true; plane.auto_navigation_mode = true; diff --git a/ArduPlane/mode_thermal.cpp b/ArduPlane/mode_thermal.cpp index ed5377c4a4..3611653dee 100644 --- a/ArduPlane/mode_thermal.cpp +++ b/ArduPlane/mode_thermal.cpp @@ -9,7 +9,6 @@ bool ModeThermal::_enter() return false; } - plane.throttle_allows_nudging = true; plane.auto_throttle_mode = true; plane.auto_navigation_mode = true; plane.do_loiter_at_location(); diff --git a/ArduPlane/mode_training.cpp b/ArduPlane/mode_training.cpp index f39eb3611a..23fd1c0cd4 100644 --- a/ArduPlane/mode_training.cpp +++ b/ArduPlane/mode_training.cpp @@ -3,7 +3,6 @@ bool ModeTraining::_enter() { - plane.throttle_allows_nudging = false; plane.auto_throttle_mode = false; plane.auto_navigation_mode = false; diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 6e32a5f1e9..48e4df0290 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -221,7 +221,7 @@ void Plane::calc_airspeed_errors() #endif // Bump up the target airspeed based on throttle nudging - if (throttle_allows_nudging && airspeed_nudge_cm > 0) { + if (control_mode->allows_throttle_nudging() && airspeed_nudge_cm > 0) { target_airspeed_cm += airspeed_nudge_cm; }