diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 8db5b0d35b..6f8fef929a 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -38,7 +38,7 @@ float Plane::get_speed_scaler(void) */ bool Plane::stick_mixing_enabled(void) { - if (auto_throttle_mode) { + if (auto_throttle_mode && auto_navigation_mode) { // we're in an auto mode. Check the stick mixing flag if (g.stick_mixing != STICK_MIXING_DISABLED && geofence_stickmixing() && diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index dd049fabef..5a23ec71aa 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -560,10 +560,14 @@ private: // true if we are in an auto-throttle mode, which means // we need to run the speed/height controller - bool auto_throttle_mode; + 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; + bool throttle_suppressed:1; // reduce throttle to eliminate battery over-current int8_t throttle_watt_limit_max; diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 367901af2a..fdb195c5c9 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -231,8 +231,11 @@ void Plane::read_radio() // in rudder only mode we discard rudder input and get target // attitude from the roll channel. rudder_input = 0; - } else { + } else if (stick_mixing_enabled()) { rudder_input = channel_rudder->get_control_in(); + } else { + // no stick mixing + rudder_input = 0; } // check for transmitter tuning changes diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index b2e67bb95b..af6a773c4e 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -377,6 +377,7 @@ void Plane::set_mode(enum FlightMode mode) { case INITIALISING: auto_throttle_mode = true; + auto_navigation_mode = false; break; case MANUAL: @@ -384,21 +385,25 @@ void Plane::set_mode(enum FlightMode mode) case TRAINING: case FLY_BY_WIRE_A: auto_throttle_mode = false; + auto_navigation_mode = false; break; case AUTOTUNE: auto_throttle_mode = false; + auto_navigation_mode = false; autotune_start(); break; case ACRO: auto_throttle_mode = false; + auto_navigation_mode = false; acro_state.locked_roll = false; acro_state.locked_pitch = false; break; case CRUISE: auto_throttle_mode = true; + auto_navigation_mode = false; cruise_state.locked_heading = false; cruise_state.lock_timer_ms = 0; set_target_altitude_current(); @@ -406,17 +411,20 @@ void Plane::set_mode(enum FlightMode mode) case FLY_BY_WIRE_B: auto_throttle_mode = true; + auto_navigation_mode = false; set_target_altitude_current(); break; case CIRCLE: // the altitude to circle at is taken from the current altitude auto_throttle_mode = true; + auto_navigation_mode = true; next_WP_loc.alt = current_loc.alt; break; case AUTO: auto_throttle_mode = true; + auto_navigation_mode = true; if (quadplane.available() && quadplane.enable == 2) { auto_state.vtol_mode = true; } else { @@ -429,17 +437,20 @@ void Plane::set_mode(enum FlightMode mode) case RTL: auto_throttle_mode = true; + auto_navigation_mode = true; prev_WP_loc = current_loc; do_RTL(get_RTL_altitude()); break; case LOITER: auto_throttle_mode = true; + auto_navigation_mode = true; do_loiter_at_location(); break; case GUIDED: auto_throttle_mode = true; + auto_navigation_mode = true; guided_throttle_passthru = false; /* when entering guided mode we set the target as the current @@ -454,6 +465,7 @@ void Plane::set_mode(enum FlightMode mode) case QLOITER: case QLAND: case QRTL: + auto_navigation_mode = false; if (!quadplane.init_mode()) { control_mode = previous_mode; } else {