Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Paul Riseborough 2023-06-02 16:08:20 +10:00 committed by Andrew Tridgell
parent 8bec6c9ff4
commit c92f71842b
2 changed files with 11 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/Plane.h
View File

@ -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;

17
ArduPlane/navigation.cpp
View File

@ -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;
}
}