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

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

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

4
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

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