Plane: Added extra parameter to specify at what speed in auto-takeoff throttle should engage

This adds TKOFF_THR_MINSPD in m/s
This commit is contained in:
Michael Warren 2013-02-09 21:40:16 +10:00 committed by Andrew Tridgell
parent 87c6545ac6
commit 9697ff5d6d
3 changed files with 17 additions and 2 deletions

View File

@ -387,7 +387,7 @@ static void throttle_slew_limit(int16_t last_throttle)
* AND
* 2 - Our reported altitude is within 10 meters of the home altitude.
* 3 - Our reported speed is under 5 meters per second.
* 4 - We are not performing a takeoff in Auto mode
* 4 - We are not performing a takeoff in Auto mode or takeoff speed not yet reached
* OR
* 5 - Home location is not set
*/
@ -403,7 +403,11 @@ static bool suppress_throttle(void)
return false;
}
if (control_mode==AUTO && takeoff_complete == false) {
if (control_mode==AUTO &&
takeoff_complete == false &&
g_gps != NULL &&
g_gps->status() == GPS::GPS_OK &&
g_gps->ground_speed >= g.takeoff_throttle_min_speed*100) {
// we're in auto takeoff
throttle_suppressed = false;
return false;

View File

@ -78,6 +78,7 @@ public:
k_param_throttle_nudge,
k_param_alt_offset,
k_param_ins, // libraries/AP_InertialSensor variables
k_param_takeoff_throttle_min_speed,
// 110: Telemetry control
//
@ -354,6 +355,7 @@ public:
AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
AP_Int8 stick_mixing;
AP_Int8 rudder_steer;
AP_Float takeoff_throttle_min_speed;
// RC channels
RC_Channel channel_roll;

View File

@ -88,6 +88,15 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Advanced
GSCALAR(stick_mixing, "STICK_MIXING", 1),
// @Param: TKOFF_THR_MINSPD
// @DisplayName: Takeoff throttle min speed
// @Description: Minimum GPS ground speed in m/s before un-suppressing throttle in auto-takeoff. This is meant to be used for catapult launches where you want the motor to engage only after the plane leaves the catapult. Note that the GPS velocity will lag the real velocity by about 0.5seconds.
// @Units: m/s
// @Range: 0 30
// @Increment: 0.1
// @User: User
GSCALAR(takeoff_throttle_min_speed, "TKOFF_THR_MINSPD", 0),
// @Param: RUDDER_STEER
// @DisplayName: Rudder steering on takeoff and landing
// @Description: When enabled, only rudder will be used for steering during takeoff and landing, with the ailerons used to hold the plane level