Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: prevent a float exception at pitch 90

Browse Source 
this can happen with a tailsitter
This commit is contained in:
Andrew Tridgell 2017-10-30 15:19:01 +11:00
parent 1ad929c56e
commit 9709bca7b4
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduPlane/navigation.cpp
View File

@ -156,9 +156,11 @@ void Plane::calc_gndspeed_undershoot()
Vector2f gndVel = ahrs.groundspeed_vector(); Vector2f gndVel = ahrs.groundspeed_vector();
const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned(); const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x); Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x);
yawVect.normalize(); if (!yawVect.is_zero()) {
float gndSpdFwd = yawVect * gndVel; yawVect.normalize();
groundspeed_undershoot = (aparm.min_gndspeed_cm > 0) ? (aparm.min_gndspeed_cm - gndSpdFwd*100) : 0; float gndSpdFwd = yawVect * gndVel;
groundspeed_undershoot = (aparm.min_gndspeed_cm > 0) ? (aparm.min_gndspeed_cm - gndSpdFwd*100) : 0;
}
} }
} }