mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 04:03:59 -04:00
Plane: added TKOFF_THR_MINACC option
this is used for triggering auto takeoff with a hand launched plane
This commit is contained in:
parent
9697ff5d6d
commit
0a385cc0ff
@ -379,6 +379,32 @@ static void throttle_slew_limit(int16_t last_throttle)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
check for automatic takeoff conditions being met
|
||||||
|
*/
|
||||||
|
static bool auto_takeoff_check(void)
|
||||||
|
{
|
||||||
|
if (g_gps == NULL || g_gps->status() != GPS::GPS_OK) {
|
||||||
|
// no auto takeoff without GPS lock
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (g_gps->ground_speed < g.takeoff_throttle_min_speed*100.0f) {
|
||||||
|
// we haven't reached the minimum ground speed
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (g.takeoff_throttle_min_accel > 0.0f &&
|
||||||
|
(ins.get_accel().x < g.takeoff_throttle_min_accel) &&
|
||||||
|
ahrs.pitch_sensor > -3000 && ahrs.pitch_sensor < 4500 &&
|
||||||
|
abs(ahrs.roll_sensor) < 3000) {
|
||||||
|
// we haven't reached the minimum acceleration or we are not
|
||||||
|
// anywhere near flat. Thanks to Chris Miser for this
|
||||||
|
// suggestion
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
// we're good for takeoff
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
/* We want to supress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
|
/* We want to supress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
|
||||||
|
|
||||||
@ -387,7 +413,7 @@ static void throttle_slew_limit(int16_t last_throttle)
|
|||||||
* AND
|
* AND
|
||||||
* 2 - Our reported altitude is within 10 meters of the home altitude.
|
* 2 - Our reported altitude is within 10 meters of the home altitude.
|
||||||
* 3 - Our reported speed is under 5 meters per second.
|
* 3 - Our reported speed is under 5 meters per second.
|
||||||
* 4 - We are not performing a takeoff in Auto mode or takeoff speed not yet reached
|
* 4 - We are not performing a takeoff in Auto mode or takeoff speed/accel not yet reached
|
||||||
* OR
|
* OR
|
||||||
* 5 - Home location is not set
|
* 5 - Home location is not set
|
||||||
*/
|
*/
|
||||||
@ -403,11 +429,7 @@ static bool suppress_throttle(void)
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (control_mode==AUTO &&
|
if (control_mode==AUTO && takeoff_complete == false && auto_takeoff_check()) {
|
||||||
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
|
// we're in auto takeoff
|
||||||
throttle_suppressed = false;
|
throttle_suppressed = false;
|
||||||
return false;
|
return false;
|
||||||
|
@ -79,6 +79,7 @@ public:
|
|||||||
k_param_alt_offset,
|
k_param_alt_offset,
|
||||||
k_param_ins, // libraries/AP_InertialSensor variables
|
k_param_ins, // libraries/AP_InertialSensor variables
|
||||||
k_param_takeoff_throttle_min_speed,
|
k_param_takeoff_throttle_min_speed,
|
||||||
|
k_param_takeoff_throttle_min_accel,
|
||||||
|
|
||||||
// 110: Telemetry control
|
// 110: Telemetry control
|
||||||
//
|
//
|
||||||
@ -356,6 +357,7 @@ public:
|
|||||||
AP_Int8 stick_mixing;
|
AP_Int8 stick_mixing;
|
||||||
AP_Int8 rudder_steer;
|
AP_Int8 rudder_steer;
|
||||||
AP_Float takeoff_throttle_min_speed;
|
AP_Float takeoff_throttle_min_speed;
|
||||||
|
AP_Float takeoff_throttle_min_accel;
|
||||||
|
|
||||||
// RC channels
|
// RC channels
|
||||||
RC_Channel channel_roll;
|
RC_Channel channel_roll;
|
||||||
|
@ -97,6 +97,15 @@ const AP_Param::Info var_info[] PROGMEM = {
|
|||||||
// @User: User
|
// @User: User
|
||||||
GSCALAR(takeoff_throttle_min_speed, "TKOFF_THR_MINSPD", 0),
|
GSCALAR(takeoff_throttle_min_speed, "TKOFF_THR_MINSPD", 0),
|
||||||
|
|
||||||
|
// @Param: TKOFF_THR_MINACC
|
||||||
|
// @DisplayName: Takeoff throttle min acceleration
|
||||||
|
// @Description: Minimum forward acceleration in m/s/s before un-suppressing throttle in auto-takeoff. This is meant to be used for hand launches with a tractor style (front engine) plane. If this is set then the auto takeoff will only trigger if the pitch of the plane is between -30 and +45 degrees, and the roll is less than 30 degrees. This makes it less likely it will trigger due to carrying the plane with the nose down.
|
||||||
|
// @Units: m/s/s
|
||||||
|
// @Range: 0 30
|
||||||
|
// @Increment: 0.1
|
||||||
|
// @User: User
|
||||||
|
GSCALAR(takeoff_throttle_min_accel, "TKOFF_THR_MINACC", 0),
|
||||||
|
|
||||||
// @Param: RUDDER_STEER
|
// @Param: RUDDER_STEER
|
||||||
// @DisplayName: Rudder steering on takeoff and landing
|
// @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
|
// @Description: When enabled, only rudder will be used for steering during takeoff and landing, with the ailerons used to hold the plane level
|
||||||
|
Loading…
Reference in New Issue
Block a user