diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 20df5f01a0..af0303506c 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -365,19 +365,6 @@ static void set_servos(void) g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX)); } -#if THROTTLE_OUT == 0 - g.channel_throttle.servo_out = 0; -#else - // convert 0 to 100% into PWM - g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get()); - - if (suppress_throttle()) { - g.channel_throttle.servo_out = 0; - } - -#endif - - if (control_mode >= FLY_BY_WIRE_B) { /* only do throttle slew limiting in modes where throttle * control is automatic */ @@ -402,8 +389,28 @@ static void set_servos(void) g.channel_roll.calc_pwm(); g.channel_pitch.calc_pwm(); } - g.channel_throttle.calc_pwm(); g.channel_rudder.calc_pwm(); + +#if THROTTLE_OUT == 0 + g.channel_throttle.servo_out = 0; +#else + // convert 0 to 100% into PWM + g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, + g.throttle_min.get(), + g.throttle_max.get()); + + if (suppress_throttle()) { + g.channel_throttle.servo_out = 0; + if (g.throttle_suppress_manual) { + // manual pass through of throttle while throttle is suppressed + g.channel_throttle.radio_out = g.channel_throttle.radio_in; + } else { + g.channel_throttle.calc_pwm(); + } + } else { + g.channel_throttle.calc_pwm(); + } +#endif } // Auto flap deployment diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 614998aa80..321917f6b2 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -160,6 +160,7 @@ public: k_param_long_fs_action, k_param_gcs_heartbeat_fs_enabled, k_param_throttle_slewrate, + k_param_throttle_suppress_manual, // // 200: Feed-forward gains @@ -275,6 +276,7 @@ public: AP_Int8 throttle_min; AP_Int8 throttle_max; AP_Int8 throttle_slewrate; + AP_Int8 throttle_suppress_manual; AP_Int8 throttle_fs_enabled; AP_Int16 throttle_fs_value; AP_Int8 throttle_cruise; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index abb81f89f3..201b5f4f2c 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -281,6 +281,13 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(throttle_slewrate, "THR_SLEWRATE", THROTTLE_SLEW_LIMIT), + // @Param: THR_SUPP_MAN + // @DisplayName: Throttle suppress manual passthru + // @Description: When throttle is supressed in auto mode it is normally forced to zero. If you enable this option, then while suppressed it will be manual throttle. This is useful on petrol engines to hold the idle throttle manually while waiting for takeoff + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + GSCALAR(throttle_suppress_manual,"THR_SUPP_MAN", 0), + // @Param: THR_FAILSAFE // @DisplayName: Throttle Failsafe Enable // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel