From 055d8fe7aaf1d11f88491dcb9b92957394c44450 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 24 May 2014 22:19:50 +1000 Subject: [PATCH] Plane: added TKOFF_THR_MAX parameter --- ArduPlane/ArduPlane.pde | 2 +- ArduPlane/Parameters.h | 2 ++ ArduPlane/Parameters.pde | 11 ++++++++++- ArduPlane/takeoff.pde | 11 +++++++++++ 4 files changed, 24 insertions(+), 2 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 06142f2fcd..5235de1d12 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1118,7 +1118,7 @@ static void handle_auto_mode(void) takeoff_calc_pitch(); // max throttle for takeoff - channel_throttle->servo_out = aparm.throttle_max; + channel_throttle->servo_out = takeoff_throttle(); break; case MAV_CMD_NAV_LAND: diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 6f1284acca..0ac23a2126 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -114,6 +114,7 @@ public: k_param_takeoff_tdrag_speed1, k_param_takeoff_rotate_speed, k_param_takeoff_throttle_slewrate, + k_param_takeoff_throttle_max, // 100: Arming parameters k_param_arming = 100, @@ -437,6 +438,7 @@ public: AP_Float takeoff_tdrag_speed1; AP_Float takeoff_rotate_speed; AP_Int8 takeoff_throttle_slewrate; + AP_Int8 takeoff_throttle_max; AP_Int8 level_roll_limit; AP_Int8 flapin_channel; AP_Int8 flaperon_output; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 79aab181a6..0aa29d27fa 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -375,13 +375,22 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: THR_MAX // @DisplayName: Maximum Throttle - // @Description: The maximum throttle setting to which the autopilot will apply. + // @Description: The maximum throttle setting as a percentage which the autopilot will apply. // @Units: Percent // @Range: 0 100 // @Increment: 1 // @User: Standard ASCALAR(throttle_max, "THR_MAX", THROTTLE_MAX), + // @Param: TKOFF_THR_MAX + // @DisplayName: Maximum Throttle for takeoff + // @Description: The maximum throttle setting during automatic takeoff. If this is zero then THR_MAX is used for takeoff as well. + // @Units: Percent + // @Range: 0 100 + // @Increment: 1 + // @User: Advanced + GSCALAR(takeoff_throttle_max, "TKOFF_THR_MAX", 0), + // @Param: THR_SLEWRATE // @DisplayName: Throttle slew rate // @Description: maximum percentage change in throttle per second. A setting of 10 means to not change the throttle by more than 10% of the full throttle range in one second. diff --git a/ArduPlane/takeoff.pde b/ArduPlane/takeoff.pde index 300eb58194..d588659ed6 100644 --- a/ArduPlane/takeoff.pde +++ b/ArduPlane/takeoff.pde @@ -155,3 +155,14 @@ static int8_t takeoff_tail_hold(void) // we are holding the tail down return g.takeoff_tdrag_elevator; } + +/* + return throttle percentage for takeoff + */ +static uint8_t takeoff_throttle(void) +{ + if (g.takeoff_throttle_max != 0) { + return g.takeoff_throttle_max; + } + return aparm.throttle_max; +}