Plane: added Q_ASSIST_ALT parameter

this allows for quadplane assistance in fixed wing modes when the
aircraft drops below a given altitude. This could help save an
aircraft that is flying badly in fixed wing mode
This commit is contained in:
Andrew Tridgell 2019-11-01 21:44:23 +11:00 committed by Randy Mackay
parent b4abab2add
commit 90d15af4ae
2 changed files with 42 additions and 2 deletions

View File

@ -461,6 +461,15 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
// @User: Advanced
AP_GROUPINFO("TKOFF_ARSP_LIM", 15, QuadPlane, maximum_takeoff_airspeed, 0),
// @Param: ASSIST_ALT
// @DisplayName: Quadplane assistance altitude
// @Description: This is the altitude below which quadplane assistance will be triggered. This acts the same way as Q_ASSIST_ANGLE and Q_ASSIST_SPEED, but triggers if the aircraft drops below the given altitude while the VTOL motors are not running. A value of zero disables this feature. The altutude is calculated as being above ground level. The height above ground is given from a Lidar used if available and RNGFND_LANDING=1. Otherwise it comes from terrain data if TERRAIN_FOLLOW=1 and comes from height above home otherwise.
// @Units: m
// @Range: 0 120
// @Increment: 1
// @User: Standard
AP_GROUPINFO("ASSIST_ALT", 16, QuadPlane, assist_alt, 0),
AP_GROUPEND
};
@ -1377,6 +1386,7 @@ bool QuadPlane::assistance_needed(float aspeed)
angle_error_start_ms = 0;
return false;
}
if (aspeed < assist_speed) {
// assistance due to Q_ASSIST_SPEED
in_angle_assist = false;
@ -1384,6 +1394,31 @@ bool QuadPlane::assistance_needed(float aspeed)
return true;
}
const uint32_t now = AP_HAL::millis();
/*
optional assistance when altitude is too close to the ground
*/
if (assist_alt > 0) {
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
if (height_above_ground < assist_alt) {
if (alt_error_start_ms == 0) {
alt_error_start_ms = now;
}
if (now - alt_error_start_ms > 500) {
// we've been below assistant alt for 0.5s
if (!in_alt_assist) {
in_alt_assist = true;
gcs().send_text(MAV_SEVERITY_INFO, "Alt assist %.1fm", height_above_ground);
}
return true;
}
} else {
in_alt_assist = false;
alt_error_start_ms = 0;
}
}
if (assist_angle <= 0) {
in_angle_assist = false;
angle_error_start_ms = 0;
@ -1412,7 +1447,7 @@ bool QuadPlane::assistance_needed(float aspeed)
in_angle_assist = false;
return false;
}
const uint32_t now = AP_HAL::millis();
if (angle_error_start_ms == 0) {
angle_error_start_ms = now;
}

View File

@ -280,6 +280,11 @@ private:
AP_Int8 assist_angle;
uint32_t angle_error_start_ms;
// altitude to trigger assistance
AP_Int16 assist_alt;
uint32_t alt_error_start_ms;
bool in_alt_assist;
// maximum yaw rate in degrees/second
AP_Float yaw_rate_max;