From c92f71842baaa3eae1ad2e2116204ad0c5b55543 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 2 Jun 2023 16:08:20 +1000 Subject: [PATCH] ArduPlane: Enable ground speed undershoot correction without GPS The code had a bug where if GPS fix was lost, the demanded airspeed would be set to the measured or estimated airspeed causing unpredictable variations in the demanded airspeed. This patch prevents the minimum ground speed protection speed up from running if the ground speed undershoot cannot be calculated. This patch extends the range of conditions over which the minimum ground speed functionality can be used by enabling the ground speed undershoot to be calculated when the navigation system is able to estimate velocity. --- ArduPlane/Plane.h | 1 + ArduPlane/navigation.cpp | 17 ++++++++++------- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 532035a1a3..504ea7b840 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -387,6 +387,7 @@ private: // Ground speed // The amount current ground speed is below min ground speed. Centimeters per second int32_t groundspeed_undershoot; + bool groundspeed_undershoot_is_valid; // Difference between current altitude and desired altitude. Centimeters int32_t altitude_error_cm; diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 5e7b510730..e06a80e945 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -244,9 +244,10 @@ void Plane::calc_airspeed_errors() // but only when this is faster than the target airspeed commanded // above. if (control_mode->does_auto_throttle() && - aparm.min_gndspeed_cm > 0 && - control_mode != &mode_circle) { - int32_t min_gnd_target_airspeed = airspeed_measured*100 + groundspeed_undershoot; + groundspeed_undershoot_is_valid && + control_mode != &mode_circle) { + float EAS_undershoot = (int32_t)((float)groundspeed_undershoot / ahrs.get_EAS2TAS()); + int32_t min_gnd_target_airspeed = airspeed_measured*100 + EAS_undershoot; if (min_gnd_target_airspeed > target_airspeed_cm) { target_airspeed_cm = min_gnd_target_airspeed; } @@ -276,16 +277,18 @@ void Plane::calc_gndspeed_undershoot() { // Use the component of ground speed in the forward direction // This prevents flyaway if wind takes plane backwards - if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) { - Vector2f gndVel = ahrs.groundspeed_vector(); + Vector3f velNED; + if (ahrs.have_inertial_nav() && ahrs.get_velocity_NED(velNED)) { const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned(); Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x); if (!yawVect.is_zero()) { yawVect.normalize(); - float gndSpdFwd = yawVect * gndVel; - groundspeed_undershoot = (aparm.min_gndspeed_cm > 0) ? (aparm.min_gndspeed_cm - gndSpdFwd*100) : 0; + float gndSpdFwd = yawVect * velNED.xy(); + groundspeed_undershoot_is_valid = aparm.min_gndspeed_cm > 0; + groundspeed_undershoot = groundspeed_undershoot_is_valid ? (aparm.min_gndspeed_cm - gndSpdFwd*100) : 0; } } else { + groundspeed_undershoot_is_valid = false; groundspeed_undershoot = 0; } }