From fef8b37b11511c8cd5e9dab90e005e32fad4fc3e Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar do Carmo Lucas" Date: Mon, 14 Dec 2020 13:47:05 +0100 Subject: [PATCH] Plane: Address Randy review comments --- ArduPlane/Attitude.cpp | 9 ++++++--- ArduPlane/radio.cpp | 13 ++++++++----- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 8b09bdb318..f39f309fe3 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -54,13 +54,16 @@ float Plane::get_speed_scaler(void) */ bool Plane::stick_mixing_enabled(void) { +#if AC_FENCE == ENABLED + const bool stickmixing = fence_stickmixing(); +#else + const bool stickmixing = true; +#endif 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 && - #if AC_FENCE == ENABLED - fence_stickmixing() && - #endif + stickmixing && failsafe.state == FAILSAFE_NONE && !rc_failsafe_active()) { // we're in an auto mode, and haven't triggered failsafe diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index b2981791a9..9ae91e51f7 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -186,13 +186,16 @@ void Plane::read_radio() control_failsafe(); +#if AC_FENCE == ENABLED + const bool stickmixing = fence_stickmixing(); +#else + const bool stickmixing = true; +#endif airspeed_nudge_cm = 0; throttle_nudge = 0; - if (g.throttle_nudge && channel_throttle->get_control_in() > 50 - #if AC_FENCE == ENABLED - && fence_stickmixing() - #endif - ) { + if (g.throttle_nudge + && channel_throttle->get_control_in() > 50 + && stickmixing) { float nudge = (channel_throttle->get_control_in() - 50) * 0.02f; if (ahrs.airspeed_sensor_enabled()) { airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge;