Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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

8
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
bool NavEKF3_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;