From 92cd631ce58e4c0bb8696fde862412e89f27fcd1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 26 Nov 2012 23:29:00 +1100 Subject: [PATCH] Plane: added THR_PASS_STAB parameter this allows direct passthru of throttle in STABILIZE and FBWA, which is useful for nitro planes wher you have a throttle cut switch that drops the throttle below normal minimum. --- ArduPlane/Attitude.pde | 7 +++++++ ArduPlane/Parameters.h | 2 ++ ArduPlane/Parameters.pde | 7 +++++++ 3 files changed, 16 insertions(+) diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 6fc9691e61..454c1f9956 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -454,6 +454,7 @@ static void set_servos(void) g.throttle_max.get()); if (suppress_throttle()) { + // throttle is suppressed in auto mode g.channel_throttle.servo_out = 0; if (g.throttle_suppress_manual) { // manual pass through of throttle while throttle is suppressed @@ -461,7 +462,13 @@ static void set_servos(void) } else { g.channel_throttle.calc_pwm(); } + } else if (g.throttle_passthru_stabilize && + (control_mode == STABILIZE || control_mode == FLY_BY_WIRE_A)) { + // manual pass through of throttle while in FBWA or + // STABILIZE mode with THR_PASS_STAB set + g.channel_throttle.radio_out = g.channel_throttle.radio_in; } else { + // normal throttle calculation based on servo_out g.channel_throttle.calc_pwm(); } #endif diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 7cde3bb08f..5230d21819 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -170,6 +170,7 @@ public: k_param_gcs_heartbeat_fs_enabled, k_param_throttle_slewrate, k_param_throttle_suppress_manual, + k_param_throttle_passthru_stabilize, // // 200: Feed-forward gains @@ -286,6 +287,7 @@ public: AP_Int8 throttle_max; AP_Int8 throttle_slewrate; AP_Int8 throttle_suppress_manual; + AP_Int8 throttle_passthru_stabilize; 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 d90f5d1ad2..194a7f190e 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -288,6 +288,13 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Advanced GSCALAR(throttle_suppress_manual,"THR_SUPP_MAN", 0), + // @Param: THR_PASS_STAB + // @DisplayName: Throttle passthru in stabilize + // @Description: If this is set then when in STABILIZE or FBWA mode the throttle is a direct passthru from the transmitter. This means the THR_MIN and THR_MAX settings are not used in these modes. This is useful for petrol engines where you setup a throttle cut switch that suppresses the throttle below the normal minimum. + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + GSCALAR(throttle_passthru_stabilize,"THR_PASS_STAB", 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