mirror of https://github.com/ArduPilot/ardupilot
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:
parent
aad00db692
commit
8ab1e67b13
|
@ -461,6 +461,15 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
|
||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
AP_GROUPINFO("TKOFF_ARSP_LIM", 15, QuadPlane, maximum_takeoff_airspeed, 0),
|
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
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -1377,6 +1386,7 @@ bool QuadPlane::assistance_needed(float aspeed)
|
||||||
angle_error_start_ms = 0;
|
angle_error_start_ms = 0;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (aspeed < assist_speed) {
|
if (aspeed < assist_speed) {
|
||||||
// assistance due to Q_ASSIST_SPEED
|
// assistance due to Q_ASSIST_SPEED
|
||||||
in_angle_assist = false;
|
in_angle_assist = false;
|
||||||
|
@ -1384,6 +1394,31 @@ bool QuadPlane::assistance_needed(float aspeed)
|
||||||
return true;
|
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) {
|
if (assist_angle <= 0) {
|
||||||
in_angle_assist = false;
|
in_angle_assist = false;
|
||||||
angle_error_start_ms = 0;
|
angle_error_start_ms = 0;
|
||||||
|
@ -1412,7 +1447,7 @@ bool QuadPlane::assistance_needed(float aspeed)
|
||||||
in_angle_assist = false;
|
in_angle_assist = false;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
const uint32_t now = AP_HAL::millis();
|
|
||||||
if (angle_error_start_ms == 0) {
|
if (angle_error_start_ms == 0) {
|
||||||
angle_error_start_ms = now;
|
angle_error_start_ms = now;
|
||||||
}
|
}
|
||||||
|
|
|
@ -280,6 +280,11 @@ private:
|
||||||
AP_Int8 assist_angle;
|
AP_Int8 assist_angle;
|
||||||
uint32_t angle_error_start_ms;
|
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
|
// maximum yaw rate in degrees/second
|
||||||
AP_Float yaw_rate_max;
|
AP_Float yaw_rate_max;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue