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

AP_NavEKF2: ensure wind estimation from airspeed can be used on its own

This commit is contained in:
Andy Piper 2022-12-01 19:09:56 +00:00 committed by Andrew Tridgell
parent 56ad3887f7
commit d3d841ec98
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -172,12 +172,14 @@ void NavEKF2_core::getVelNED(Vector3f &vel) const
// returns false if estimate is unavailable
bool NavEKF2_core::getAirSpdVec(Vector3f &vel) const
{
if (inhibitWindStates || PV_AidingMode == AID_NONE) {
if (PV_AidingMode == AID_NONE) {
return false;
}
vel = (outputDataNew.velocity + velOffsetNED).tofloat();
vel.x -= stateStruct.wind_vel.x;
vel.y -= stateStruct.wind_vel.y;
if (!inhibitWindStates) {
vel.x -= stateStruct.wind_vel.x;
vel.y -= stateStruct.wind_vel.y;
}
Matrix3f Tnb; // rotation from nav to body frame
outputDataNew.quat.inverse().rotation_matrix(Tnb);
vel = Tnb * vel;