From f6cb1b5ad6ec9feea6c1f9e5872138a3827bdc1b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 23 Dec 2020 15:56:54 +1100 Subject: [PATCH] Plane: use a method on Mode for auto-navigation-mode Avoids the state getting stale, which it will with a failed attempt to go into qautotune, for example. --- ArduPlane/Attitude.cpp | 2 +- ArduPlane/Plane.h | 4 ---- ArduPlane/mode.cpp | 2 +- ArduPlane/mode.h | 18 ++++++++++++++++++ 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 - 22 files changed, 20 insertions(+), 24 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 6257684b9d..57f8f9c71c 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -54,7 +54,7 @@ float Plane::get_speed_scaler(void) */ bool Plane::stick_mixing_enabled(void) { - if (auto_throttle_mode && auto_navigation_mode) { + if (auto_throttle_mode && plane.control_mode->does_auto_navigation()) { // we're in an auto mode. Check the stick mixing flag if (g.stick_mixing != STICK_MIXING_DISABLED && g.stick_mixing != STICK_MIXING_VTOL_YAW && diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 8d4388e971..2471f6bf05 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -576,10 +576,6 @@ private: // we need to run the speed/height controller bool auto_throttle_mode:1; - // 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 controls throttle suppression in auto modes bool throttle_suppressed; diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index e3a63a6c48..fe57a6a092 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -73,7 +73,7 @@ bool Mode::enter() // start with throttle suppressed in auto_throttle modes plane.throttle_suppressed = plane.auto_throttle_mode; #if HAL_ADSB_ENABLED - plane.adsb.set_is_auto_mode(plane.auto_navigation_mode); + plane.adsb.set_is_auto_mode(does_auto_navigation()); #endif // reset steering integrator on mode change diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index c0f07e594c..bbef6f31fa 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -87,6 +87,10 @@ public: // depending on airspeed_nudge_cm virtual bool allows_throttle_nudging() const { return false; } + // true if the mode sets the vehicle destination, which controls + // whether control input is ignored with STICK_MIXING=0 + virtual bool does_auto_navigation() const { return false; } + protected: // subclasses override this to perform checks before entering the mode @@ -128,6 +132,8 @@ public: bool allows_throttle_nudging() const override { return true; } + bool does_auto_navigation() const override { return true; } + protected: bool _enter() override; @@ -169,6 +175,8 @@ public: bool allows_throttle_nudging() const override { return true; } + bool does_auto_navigation() const override { return true; } + protected: bool _enter() override; @@ -185,6 +193,8 @@ public: // methods that affect movement of the vehicle in this mode void update() override; + bool does_auto_navigation() const override { return true; } + protected: bool _enter() override; @@ -206,6 +216,8 @@ public: bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc); bool isHeadingLinedUp_cd(const int32_t bearing_cd); + bool does_auto_navigation() const override { return true; } + protected: bool _enter() override; @@ -244,6 +256,8 @@ public: bool allows_throttle_nudging() const override { return true; } + bool does_auto_navigation() const override { return true; } + protected: bool _enter() override; @@ -536,6 +550,8 @@ public: bool allows_throttle_nudging() const override { return true; } + bool does_auto_navigation() const override { return true; } + // var_info for holding parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -571,6 +587,8 @@ public: bool allows_throttle_nudging() const override { return true; } + bool does_auto_navigation() const override { return true; } + protected: bool exit_heading_aligned() const; diff --git a/ArduPlane/mode_acro.cpp b/ArduPlane/mode_acro.cpp index 31449e3d81..182261811e 100644 --- a/ArduPlane/mode_acro.cpp +++ b/ArduPlane/mode_acro.cpp @@ -4,7 +4,6 @@ bool ModeAcro::_enter() { plane.auto_throttle_mode = false; - plane.auto_navigation_mode = false; plane.acro_state.locked_roll = false; plane.acro_state.locked_pitch = false; diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 467929d0aa..3488bc2223 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -4,7 +4,6 @@ bool ModeAuto::_enter() { plane.auto_throttle_mode = true; - plane.auto_navigation_mode = true; if (plane.quadplane.available() && plane.quadplane.enable == 2) { plane.auto_state.vtol_mode = true; } else { diff --git a/ArduPlane/mode_autotune.cpp b/ArduPlane/mode_autotune.cpp index e4b8b120f3..fe8435a90c 100644 --- a/ArduPlane/mode_autotune.cpp +++ b/ArduPlane/mode_autotune.cpp @@ -4,7 +4,6 @@ bool ModeAutoTune::_enter() { plane.auto_throttle_mode = false; - plane.auto_navigation_mode = false; plane.autotune_start(); return true; diff --git a/ArduPlane/mode_circle.cpp b/ArduPlane/mode_circle.cpp index 2d51839a26..2b5dd94078 100644 --- a/ArduPlane/mode_circle.cpp +++ b/ArduPlane/mode_circle.cpp @@ -5,7 +5,6 @@ bool ModeCircle::_enter() { // the altitude to circle at is taken from the current altitude plane.auto_throttle_mode = true; - plane.auto_navigation_mode = true; plane.next_WP_loc.alt = plane.current_loc.alt; return true; diff --git a/ArduPlane/mode_cruise.cpp b/ArduPlane/mode_cruise.cpp index a1c2d88b4c..ddd3c74682 100644 --- a/ArduPlane/mode_cruise.cpp +++ b/ArduPlane/mode_cruise.cpp @@ -4,7 +4,6 @@ bool ModeCruise::_enter() { plane.auto_throttle_mode = true; - plane.auto_navigation_mode = false; locked_heading = false; lock_timer_ms = 0; diff --git a/ArduPlane/mode_fbwa.cpp b/ArduPlane/mode_fbwa.cpp index 3c610e6c7d..06364a32b2 100644 --- a/ArduPlane/mode_fbwa.cpp +++ b/ArduPlane/mode_fbwa.cpp @@ -4,7 +4,6 @@ bool ModeFBWA::_enter() { plane.auto_throttle_mode = false; - plane.auto_navigation_mode = false; return true; } diff --git a/ArduPlane/mode_fbwb.cpp b/ArduPlane/mode_fbwb.cpp index 280ea26108..604357ca04 100644 --- a/ArduPlane/mode_fbwb.cpp +++ b/ArduPlane/mode_fbwb.cpp @@ -4,7 +4,6 @@ bool ModeFBWB::_enter() { plane.auto_throttle_mode = true; - plane.auto_navigation_mode = false; #if HAL_SOARING_ENABLED // for ArduSoar soaring_controller diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 7210e90001..2b6e402b6f 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -4,7 +4,6 @@ bool ModeGuided::_enter() { plane.auto_throttle_mode = true; - plane.auto_navigation_mode = true; plane.guided_throttle_passthru = false; /* when entering guided mode we set the target as the current diff --git a/ArduPlane/mode_initializing.cpp b/ArduPlane/mode_initializing.cpp index c7f5cc2ed1..8c7cb4ab44 100644 --- a/ArduPlane/mode_initializing.cpp +++ b/ArduPlane/mode_initializing.cpp @@ -4,7 +4,6 @@ bool ModeInitializing::_enter() { plane.auto_throttle_mode = true; - plane.auto_navigation_mode = false; return true; } diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index c6ee086394..d352b06958 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -4,7 +4,6 @@ bool ModeLoiter::_enter() { plane.auto_throttle_mode = true; - plane.auto_navigation_mode = true; plane.do_loiter_at_location(); plane.loiter_angle_reset(); diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp index 1030d53e34..0d56680bdb 100644 --- a/ArduPlane/mode_manual.cpp +++ b/ArduPlane/mode_manual.cpp @@ -4,7 +4,6 @@ bool ModeManual::_enter() { plane.auto_throttle_mode = false; - plane.auto_navigation_mode = false; return true; } diff --git a/ArduPlane/mode_qacro.cpp b/ArduPlane/mode_qacro.cpp index b2c25d6507..458861ffad 100644 --- a/ArduPlane/mode_qacro.cpp +++ b/ArduPlane/mode_qacro.cpp @@ -3,7 +3,6 @@ bool ModeQAcro::_enter() { - plane.auto_navigation_mode = false; if (!plane.quadplane.init_mode() && plane.previous_mode != nullptr) { plane.control_mode = plane.previous_mode; } else { diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index 4349764a46..dbbdf36adc 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/ArduPlane/mode_qstabilize.cpp @@ -3,7 +3,6 @@ bool ModeQStabilize::_enter() { - plane.auto_navigation_mode = false; if (!plane.quadplane.init_mode() && plane.previous_mode != nullptr) { plane.control_mode = plane.previous_mode; } else { diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index 00e4474d28..9408134084 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -4,7 +4,6 @@ bool ModeRTL::_enter() { plane.auto_throttle_mode = true; - plane.auto_navigation_mode = true; plane.prev_WP_loc = plane.current_loc; plane.do_RTL(plane.get_RTL_altitude()); plane.rtl.done_climb = false; diff --git a/ArduPlane/mode_stabilize.cpp b/ArduPlane/mode_stabilize.cpp index 23737b5b65..398a37ba48 100644 --- a/ArduPlane/mode_stabilize.cpp +++ b/ArduPlane/mode_stabilize.cpp @@ -4,7 +4,6 @@ bool ModeStabilize::_enter() { plane.auto_throttle_mode = false; - plane.auto_navigation_mode = false; return true; } diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index cd7d8a8fc9..a5bcd29e8a 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -53,7 +53,6 @@ bool ModeTakeoff::_enter() { // the altitude to circle at is taken from the current altitude plane.auto_throttle_mode = true; - plane.auto_navigation_mode = true; takeoff_started = false; diff --git a/ArduPlane/mode_thermal.cpp b/ArduPlane/mode_thermal.cpp index 3611653dee..f35b060b24 100644 --- a/ArduPlane/mode_thermal.cpp +++ b/ArduPlane/mode_thermal.cpp @@ -10,7 +10,6 @@ bool ModeThermal::_enter() } plane.auto_throttle_mode = true; - plane.auto_navigation_mode = true; plane.do_loiter_at_location(); plane.loiter_angle_reset(); diff --git a/ArduPlane/mode_training.cpp b/ArduPlane/mode_training.cpp index 23fd1c0cd4..dc7ba0f01c 100644 --- a/ArduPlane/mode_training.cpp +++ b/ArduPlane/mode_training.cpp @@ -4,7 +4,6 @@ bool ModeTraining::_enter() { plane.auto_throttle_mode = false; - plane.auto_navigation_mode = false; return true; }