From 6baaf03c8c7e563db00e60ca79e54289c8770fac Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 23 Dec 2020 16:25:35 +1100 Subject: [PATCH] Plane: move auto_throttle_mode to being a method on each mode --- ArduPlane/AP_Arming.cpp | 4 +-- ArduPlane/ArduPlane.cpp | 6 ++-- ArduPlane/Attitude.cpp | 4 +-- ArduPlane/Plane.h | 4 --- ArduPlane/afs_plane.cpp | 2 +- ArduPlane/mode.cpp | 6 ++-- ArduPlane/mode.h | 55 +++++++++++++++++++++++---------- 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 | 7 ----- ArduPlane/mode_fbwb.cpp | 2 -- ArduPlane/mode_guided.cpp | 1 - ArduPlane/mode_initializing.cpp | 10 ------ ArduPlane/mode_loiter.cpp | 1 - ArduPlane/mode_manual.cpp | 7 ----- ArduPlane/mode_qacro.cpp | 1 - ArduPlane/mode_qstabilize.cpp | 1 - ArduPlane/mode_rtl.cpp | 1 - ArduPlane/mode_stabilize.cpp | 7 ----- ArduPlane/mode_takeoff.cpp | 3 -- ArduPlane/mode_thermal.cpp | 1 - ArduPlane/mode_training.cpp | 7 ----- ArduPlane/navigation.cpp | 2 +- ArduPlane/quadplane.cpp | 10 +++--- ArduPlane/servos.cpp | 14 ++++----- 28 files changed, 63 insertions(+), 98 deletions(-) delete mode 100644 ArduPlane/mode_initializing.cpp diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 62b0af8779..d3c0c2d879 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -156,7 +156,7 @@ bool AP_Arming_Plane::arm_checks(AP_Arming::Method method) // if not in a manual throttle mode and not in CRUISE or FBWB // modes then disallow rudder arming/disarming - if (plane.auto_throttle_mode && + if (plane.control_mode->does_auto_throttle() && (plane.control_mode != &plane.mode_cruise && plane.control_mode != &plane.mode_fbwb)) { check_failed(true, "Mode not rudder-armable"); return false; @@ -254,7 +254,7 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method) } // suppress the throttle in auto-throttle modes - plane.throttle_suppressed = plane.auto_throttle_mode; + plane.throttle_suppressed = plane.control_mode->does_auto_throttle(); // if no airmode switch assigned, ensure airmode is off: if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr) { diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 79221917f3..6d0feb1454 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -169,7 +169,7 @@ void Plane::ahrs_update() */ void Plane::update_speed_height(void) { - if (auto_throttle_mode) { + if (control_mode->does_auto_throttle()) { // Call TECS 50Hz update. Note that we call this regardless of // throttle suppressed, as this needs to be running for // takeoff detection @@ -481,7 +481,7 @@ void Plane::update_alt() update_flight_stage(); - if (auto_throttle_mode && !throttle_suppressed) { + if (control_mode->does_auto_throttle() && !throttle_suppressed) { float distance_beyond_land_wp = 0; if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) { @@ -514,7 +514,7 @@ void Plane::update_alt() void Plane::update_flight_stage(void) { // Update the speed & height controller states - if (auto_throttle_mode && !throttle_suppressed) { + if (control_mode->does_auto_throttle() && !throttle_suppressed) { if (control_mode == &mode_auto) { if (quadplane.in_vtol_auto()) { set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL); diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 57f8f9c71c..109b29a54e 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 && plane.control_mode->does_auto_navigation()) { + if (control_mode->does_auto_throttle() && 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 && @@ -125,7 +125,7 @@ void Plane::stabilize_pitch(float speed_scaler) } // if LANDING_FLARE RCx_OPTION switch is set and in FW mode, manual throttle,throttle idle then set pitch to LAND_PITCH_CD if flight option FORCE_FLARE_ATTITUDE is set - if (!quadplane.in_transition() && !control_mode->is_vtol_mode() && channel_throttle->in_trim_dz() && !auto_throttle_mode && flare_mode == FlareMode::ENABLED_PITCH_TARGET) { + if (!quadplane.in_transition() && !control_mode->is_vtol_mode() && channel_throttle->in_trim_dz() && !control_mode->does_auto_throttle() && flare_mode == FlareMode::ENABLED_PITCH_TARGET) { demanded_pitch = landing.get_pitch_cd(); } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 2471f6bf05..bc9818104a 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -572,10 +572,6 @@ private: uint32_t impact_timer_ms; } crash_state; - // true if we are in an auto-throttle mode, which means - // we need to run the speed/height controller - bool auto_throttle_mode:1; - // this controls throttle suppression in auto modes bool throttle_suppressed; diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 04c12c2ab1..c58837962d 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -90,7 +90,7 @@ void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void) */ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Plane::afs_mode(void) { - if (plane.auto_throttle_mode) { + if (plane.control_mode->does_auto_throttle()) { return AP_AdvancedFailsafe::AFS_AUTO; } if (plane.control_mode == &plane.mode_manual) { diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index fe57a6a092..8c1897e289 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -71,7 +71,7 @@ bool Mode::enter() // these must be done AFTER _enter() because they use the results to set more flags // start with throttle suppressed in auto_throttle modes - plane.throttle_suppressed = plane.auto_throttle_mode; + plane.throttle_suppressed = does_auto_throttle(); #if HAL_ADSB_ENABLED plane.adsb.set_is_auto_mode(does_auto_navigation()); #endif @@ -93,8 +93,8 @@ bool Mode::is_vtol_man_throttle() const // We are a tailsitter that has fully transitioned to Q-assisted forward flight. // In this case the forward throttle directly drives the vertical throttle so // set vertical throttle state to match the forward throttle state. Confusingly the booleans are inverted, - // forward throttle uses 'auto_throttle_mode' whereas vertical uses 'is_vtol_man_throttle'. - return !plane.auto_throttle_mode; + // forward throttle uses 'does_auto_throttle' whereas vertical uses 'is_vtol_man_throttle'. + return !does_auto_throttle(); } return false; } diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index bbef6f31fa..45dac0513e 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -91,6 +91,10 @@ public: // whether control input is ignored with STICK_MIXING=0 virtual bool does_auto_navigation() 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_throttle() const { return false; } + protected: // subclasses override this to perform checks before entering the mode @@ -134,6 +138,8 @@ public: bool does_auto_navigation() const override { return true; } + bool does_auto_throttle() const override { return true; } + protected: bool _enter() override; @@ -177,6 +183,8 @@ public: bool does_auto_navigation() const override { return true; } + bool does_auto_throttle() const override { return true; } + protected: bool _enter() override; @@ -195,6 +203,8 @@ public: bool does_auto_navigation() const override { return true; } + bool does_auto_throttle() const override { return true; } + protected: bool _enter() override; @@ -216,8 +226,12 @@ public: bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc); bool isHeadingLinedUp_cd(const int32_t bearing_cd); + bool allows_throttle_nudging() const override { return true; } + bool does_auto_navigation() const override { return true; } + bool does_auto_throttle() const override { return true; } + protected: bool _enter() override; @@ -236,7 +250,6 @@ public: protected: - bool _enter() override; void _exit() override; }; @@ -258,6 +271,8 @@ public: bool does_auto_navigation() const override { return true; } + bool does_auto_throttle() const override { return true; } + protected: bool _enter() override; @@ -273,10 +288,6 @@ public: // methods that affect movement of the vehicle in this mode void update() override; - -protected: - - bool _enter() override; }; class ModeTraining : public Mode @@ -289,10 +300,6 @@ public: // methods that affect movement of the vehicle in this mode void update() override; - -protected: - - bool _enter() override; }; class ModeInitializing : public Mode @@ -308,9 +315,7 @@ public: bool allows_throttle_nudging() const override { return true; } -protected: - - bool _enter() override; + bool does_auto_throttle() const override { return true; } }; class ModeFBWA : public Mode @@ -324,10 +329,6 @@ public: // methods that affect movement of the vehicle in this mode void update() override; - bool _enter() override; - -protected: - }; class ModeFBWB : public Mode @@ -343,6 +344,8 @@ public: // methods that affect movement of the vehicle in this mode void update() override; + bool does_auto_throttle() const override { return true; } + protected: bool _enter() override; @@ -365,6 +368,8 @@ public: bool get_target_heading_cd(int32_t &target_heading); + bool does_auto_throttle() const override { return true; } + protected: bool _enter() override; @@ -390,6 +395,8 @@ public: virtual bool is_guided_mode() const override { return true; } + bool does_auto_throttle() const override { return true; } + protected: bool _enter() override; @@ -433,6 +440,8 @@ public: // methods that affect movement of the vehicle in this mode void update() override; + bool does_auto_throttle() const override { return true; } + protected: bool _enter() override; @@ -452,6 +461,8 @@ public: // methods that affect movement of the vehicle in this mode void update() override; + bool does_auto_throttle() const override { return true; } + protected: bool _enter() override; @@ -470,6 +481,8 @@ public: // methods that affect movement of the vehicle in this mode void update() override; + bool does_auto_throttle() const override { return true; } + protected: bool _enter() override; @@ -488,6 +501,8 @@ public: // methods that affect movement of the vehicle in this mode void update() override; + bool does_auto_throttle() const override { return true; } + protected: bool _enter() override; @@ -527,6 +542,8 @@ public: // methods that affect movement of the vehicle in this mode void update() override; + bool does_auto_throttle() const override { return true; } + protected: bool _enter() override; @@ -552,6 +569,8 @@ public: bool does_auto_navigation() const override { return true; } + bool does_auto_throttle() const override { return true; } + // var_info for holding parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -589,6 +608,10 @@ public: bool does_auto_navigation() const override { return true; } + // true if we are in an auto-throttle mode, which means + // we need to run the speed/height controller + bool does_auto_throttle() const override { return true; } + protected: bool exit_heading_aligned() const; diff --git a/ArduPlane/mode_acro.cpp b/ArduPlane/mode_acro.cpp index 182261811e..53d569277d 100644 --- a/ArduPlane/mode_acro.cpp +++ b/ArduPlane/mode_acro.cpp @@ -3,7 +3,6 @@ bool ModeAcro::_enter() { - plane.auto_throttle_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 3488bc2223..48cbe22791 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -3,7 +3,6 @@ bool ModeAuto::_enter() { - plane.auto_throttle_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 fe8435a90c..b6a42e9f81 100644 --- a/ArduPlane/mode_autotune.cpp +++ b/ArduPlane/mode_autotune.cpp @@ -3,7 +3,6 @@ bool ModeAutoTune::_enter() { - plane.auto_throttle_mode = false; plane.autotune_start(); return true; diff --git a/ArduPlane/mode_circle.cpp b/ArduPlane/mode_circle.cpp index 2b5dd94078..0e05a69f81 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.auto_throttle_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 ddd3c74682..a1223c5358 100644 --- a/ArduPlane/mode_cruise.cpp +++ b/ArduPlane/mode_cruise.cpp @@ -3,7 +3,6 @@ bool ModeCruise::_enter() { - plane.auto_throttle_mode = true; locked_heading = false; lock_timer_ms = 0; diff --git a/ArduPlane/mode_fbwa.cpp b/ArduPlane/mode_fbwa.cpp index 06364a32b2..917837f09d 100644 --- a/ArduPlane/mode_fbwa.cpp +++ b/ArduPlane/mode_fbwa.cpp @@ -1,13 +1,6 @@ #include "mode.h" #include "Plane.h" -bool ModeFBWA::_enter() -{ - plane.auto_throttle_mode = false; - - return true; -} - void ModeFBWA::update() { // set nav_roll and nav_pitch using sticks diff --git a/ArduPlane/mode_fbwb.cpp b/ArduPlane/mode_fbwb.cpp index 604357ca04..1478eac6fc 100644 --- a/ArduPlane/mode_fbwb.cpp +++ b/ArduPlane/mode_fbwb.cpp @@ -3,8 +3,6 @@ bool ModeFBWB::_enter() { - plane.auto_throttle_mode = true; - #if HAL_SOARING_ENABLED // for ArduSoar soaring_controller plane.g2.soaring_controller.init_cruising(); diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 2b6e402b6f..cf67a0d4e0 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -3,7 +3,6 @@ bool ModeGuided::_enter() { - plane.auto_throttle_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 deleted file mode 100644 index 8c7cb4ab44..0000000000 --- a/ArduPlane/mode_initializing.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "mode.h" -#include "Plane.h" - -bool ModeInitializing::_enter() -{ - plane.auto_throttle_mode = true; - - return true; -} - diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index d352b06958..14e725ee4a 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -3,7 +3,6 @@ bool ModeLoiter::_enter() { - plane.auto_throttle_mode = true; plane.do_loiter_at_location(); plane.loiter_angle_reset(); diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp index 0d56680bdb..56df31bf53 100644 --- a/ArduPlane/mode_manual.cpp +++ b/ArduPlane/mode_manual.cpp @@ -1,13 +1,6 @@ #include "mode.h" #include "Plane.h" -bool ModeManual::_enter() -{ - plane.auto_throttle_mode = false; - - return true; -} - void ModeManual::_exit() { if (plane.g.auto_trim > 0) { diff --git a/ArduPlane/mode_qacro.cpp b/ArduPlane/mode_qacro.cpp index 458861ffad..6e1d1ea68f 100644 --- a/ArduPlane/mode_qacro.cpp +++ b/ArduPlane/mode_qacro.cpp @@ -6,7 +6,6 @@ bool ModeQAcro::_enter() if (!plane.quadplane.init_mode() && plane.previous_mode != nullptr) { plane.control_mode = plane.previous_mode; } else { - plane.auto_throttle_mode = false; plane.auto_state.vtol_mode = true; } diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index dbbdf36adc..7fb513aa6e 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/ArduPlane/mode_qstabilize.cpp @@ -6,7 +6,6 @@ bool ModeQStabilize::_enter() if (!plane.quadplane.init_mode() && plane.previous_mode != nullptr) { plane.control_mode = plane.previous_mode; } else { - plane.auto_throttle_mode = false; plane.auto_state.vtol_mode = true; } diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index 9408134084..cb2f737e57 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -3,7 +3,6 @@ bool ModeRTL::_enter() { - plane.auto_throttle_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 398a37ba48..7414d57506 100644 --- a/ArduPlane/mode_stabilize.cpp +++ b/ArduPlane/mode_stabilize.cpp @@ -1,13 +1,6 @@ #include "mode.h" #include "Plane.h" -bool ModeStabilize::_enter() -{ - plane.auto_throttle_mode = false; - - return true; -} - void ModeStabilize::update() { plane.nav_roll_cd = 0; diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index a5bcd29e8a..a542187d96 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -51,9 +51,6 @@ ModeTakeoff::ModeTakeoff() bool ModeTakeoff::_enter() { - // the altitude to circle at is taken from the current altitude - plane.auto_throttle_mode = true; - takeoff_started = false; return true; diff --git a/ArduPlane/mode_thermal.cpp b/ArduPlane/mode_thermal.cpp index f35b060b24..1ad42fe7d4 100644 --- a/ArduPlane/mode_thermal.cpp +++ b/ArduPlane/mode_thermal.cpp @@ -9,7 +9,6 @@ bool ModeThermal::_enter() return false; } - plane.auto_throttle_mode = true; plane.do_loiter_at_location(); plane.loiter_angle_reset(); diff --git a/ArduPlane/mode_training.cpp b/ArduPlane/mode_training.cpp index dc7ba0f01c..6209aae1ec 100644 --- a/ArduPlane/mode_training.cpp +++ b/ArduPlane/mode_training.cpp @@ -1,13 +1,6 @@ #include "mode.h" #include "Plane.h" -bool ModeTraining::_enter() -{ - plane.auto_throttle_mode = false; - - return true; -} - void ModeTraining::update() { plane.training_manual_roll = false; diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 48e4df0290..108f527129 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -204,7 +204,7 @@ void Plane::calc_airspeed_errors() // Set target to current airspeed + ground speed undershoot, // but only when this is faster than the target airspeed commanded // above. - if (auto_throttle_mode && + if (control_mode->does_auto_throttle() && aparm.min_gndspeed_cm > 0 && control_mode != &mode_circle) { int32_t min_gnd_target_airspeed = airspeed_measured*100 + groundspeed_undershoot; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c16d8ddaa2..ebeb3c156a 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1482,7 +1482,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const { bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && (air_mode == AirMode::ON); if (!manual_air_mode && - plane.get_throttle_input() <= 0 && !plane.auto_throttle_mode && + plane.get_throttle_input() <= 0 && !plane.control_mode->does_auto_throttle() && plane.arming.get_rudder_arming_type() != AP_Arming::RudderArming::IS_DISABLED) { // the user may be trying to disarm return 0; @@ -1516,7 +1516,7 @@ float QuadPlane::get_desired_yaw_rate_cds(void) yaw_cds += desired_auto_yaw_rate_cds(); } bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && (air_mode == AirMode::ON); - if (plane.get_throttle_input() <= 0 && !plane.auto_throttle_mode && !manual_air_mode) { + if (plane.get_throttle_input() <= 0 && !plane.control_mode->does_auto_throttle() && !manual_air_mode) { // the user may be trying to disarm return 0; } @@ -1571,7 +1571,7 @@ void QuadPlane::set_armed(bool armed) float QuadPlane::assist_climb_rate_cms(void) const { float climb_rate; - if (plane.auto_throttle_mode) { + if (plane.control_mode->does_auto_throttle()) { // use altitude_error_cm, spread over 10s interval climb_rate = plane.altitude_error_cm * 0.1f; } else { @@ -1698,7 +1698,7 @@ bool QuadPlane::assistance_needed(float aspeed, bool have_airspeed) // return true if it is safe to provide assistance bool QuadPlane::assistance_safe() { - return hal.util->get_soft_armed() && ( (plane.auto_throttle_mode && !plane.throttle_suppressed) + return hal.util->get_soft_armed() && ( (plane.control_mode->does_auto_throttle() && !plane.throttle_suppressed) || plane.get_throttle_input()>0 || plane.is_flying() ); } @@ -2084,7 +2084,7 @@ void QuadPlane::update_throttle_suppression(void) // if we are in a fixed wing auto throttle mode and we have // unsuppressed the throttle then allow motors to run - if (plane.auto_throttle_mode && !plane.throttle_suppressed) { + if (plane.control_mode->does_auto_throttle() && !plane.throttle_suppressed) { return; } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 9e1cc89e8e..8e85f4f817 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -54,7 +54,7 @@ void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func) bool Plane::suppress_throttle(void) { #if PARACHUTE == ENABLED - if (auto_throttle_mode && parachute.release_initiated()) { + if (control_mode->does_auto_throttle() && parachute.release_initiated()) { // throttle always suppressed in auto-throttle modes after parachute release initiated throttle_suppressed = true; return true; @@ -69,7 +69,7 @@ bool Plane::suppress_throttle(void) // we've previously met a condition for unsupressing the throttle return false; } - if (!auto_throttle_mode) { + if (!control_mode->does_auto_throttle()) { // the user controls the throttle throttle_suppressed = false; return false; @@ -579,7 +579,7 @@ void Plane::set_servos_flaps(void) manual_flap_percent = channel_flap->percent_input(); } - if (auto_throttle_mode) { + if (control_mode->does_auto_throttle()) { int16_t flapSpeedSource = 0; if (ahrs.airspeed_sensor_enabled()) { flapSpeedSource = target_airspeed_cm * 0.01f; @@ -726,7 +726,7 @@ void Plane::servos_twin_engine_mix(void) */ void Plane::force_flare(void) { - if (!quadplane.in_transition() && !control_mode->is_vtol_mode() && !auto_throttle_mode && channel_throttle->in_trim_dz() && flare_mode != FlareMode::FLARE_DISABLED) { + if (!quadplane.in_transition() && !control_mode->is_vtol_mode() && !control_mode->does_auto_throttle() && channel_throttle->in_trim_dz() && flare_mode != FlareMode::FLARE_DISABLED) { int32_t tilt = -SERVO_MAX; //this is tilts up for a normal tiltrotor if (quadplane.tilt.tilt_type == QuadPlane::TILT_TYPE_BICOPTER) { tilt = 0; // this is tilts up for a Bicopter @@ -830,8 +830,8 @@ void Plane::set_servos(void) // set airbrake outputs airbrake_update(); - - if (auto_throttle_mode || + + if (control_mode->does_auto_throttle() || quadplane.in_assisted_flight() || quadplane.in_vtol_mode()) { /* only do throttle slew limiting in modes where throttle @@ -956,7 +956,7 @@ void Plane::update_throttle_hover() { void Plane::servos_auto_trim(void) { // only in auto modes and FBWA - if (!auto_throttle_mode && control_mode != &mode_fbwa) { + if (!control_mode->does_auto_throttle() && control_mode != &mode_fbwa) { return; } if (!hal.util->get_soft_armed()) {