Plane: added TKOFF_THR_MAX parameter

This commit is contained in:
Andrew Tridgell 2014-05-24 22:19:50 +10:00
parent dfedc377b1
commit 055d8fe7aa
4 changed files with 24 additions and 2 deletions

View File

@ -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:

View File

@ -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;

View File

@ -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.

View File

@ -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;
}