diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 0715079efe..fdfca2f1c2 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -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; } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 48d11957c9..abe20f3303 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -279,7 +279,12 @@ private: // angular error at which quad assistance is given 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;