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

Plane: Fixed bug in groundspeed_undershoot that could cause a fly-away in windy conditions. 

Use the component of velocity in the forward direction rather than the magnitude
This commit is contained in:
priseborough 2013-04-27 16:27:37 +10:00 committed by Andrew Tridgell
parent 663951d9a7
commit 3663426550
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduPlane/navigation.pde
View File

@ -115,10 +115,15 @@ static void calc_airspeed_errors()
static void calc_gndspeed_undershoot()
{
// Function is overkill, but here in case we want to add filtering
// later
// Use the component of ground speed in the forward direction
// This prevents flyaway if wind takes plane backwards
if (g_gps && g_gps->status() >= GPS::GPS_OK_FIX_2D) {
groundspeed_undershoot = (g.min_gndspeed_cm > 0) ? (g.min_gndspeed_cm - g_gps->ground_speed) : 0;
Vector2f gndVel = ahrs.groundspeed_vector();
Matrix3f rotMat = ahrs.get_dcm_matrix();
Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x);
yawVect = (yawVect).normalized();
float gndSpdFwd = yawVect * gndVel;
groundspeed_undershoot = (g.min_gndspeed_cm > 0) ? (g.min_gndspeed_cm - gndSpdFwd*100) : 0;
}
}