AP_NavEKF2: Add accessor function for body frame airspeed vector

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -943,6 +943,18 @@ void NavEKF2::getVelNED(int8_t instance, Vector3f &vel) const
     }
 }
 
+// return estimate of true airspeed vector in body frame in m/s for the specified instance
+// An out of range instance (eg -1) returns data for the primary instance
+// returns false if estimate is unavailable
+bool NavEKF2::getAirSpdVec(int8_t instance, Vector3f &vel) const
+{
+    if (instance < 0 || instance >= num_cores) instance = primary;
+    if (core) {
+        return core[instance].getAirSpdVec(vel);
+    }
+    return false;
+}
+
 // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
 float NavEKF2::getPosDownDerivative(int8_t instance) const
 {

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -83,6 +83,11 @@ public:
     // An out of range instance (eg -1) returns data for the primary instance
     void getVelNED(int8_t instance, Vector3f &vel) const;
 
+    // return estimate of true airspeed vector in body frame in m/s for the specified instance
+    // An out of range instance (eg -1) returns data for the primary instance
+    // returns false if estimate is unavailable
+    bool getAirSpdVec(int8_t instance, Vector3f &vel) const;
+
     // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s for the specified instance
     // An out of range instance (eg -1) returns data for the primary instance
     // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -173,6 +173,22 @@ void NavEKF2_core::getVelNED(Vector3f &vel) const
     vel = outputDataNew.velocity + velOffsetNED;
 }
 
+// return estimate of true airspeed vector in body frame in m/s
+// returns false if estimate is unavailable
+bool NavEKF2_core::getAirSpdVec(Vector3f &vel) const
+{
+    if (inhibitWindStates || PV_AidingMode == AID_NONE) {
+        return false;
+    }
+    vel = outputDataNew.velocity + velOffsetNED;
+    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;
+    return true;
+}
+
 // Return the rate of change of vertical position in the down direction (dPosD/dt) of the body frame origin in m/s
 float NavEKF2_core::getPosDownDerivative(void) const
 {

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -667,7 +667,7 @@ void NavEKF2_core::UpdateStrapdownEquationsNED()
     delVelNav  = prevTnb.mul_transpose(delVelCorrected);
     delVelNav.z += GRAVITY_MSS*imuDataDelayed.delVelDT;
 
-    // calculate the body to nav cosine matrix
+    // calculate the nav to body cosine matrix
     stateStruct.quat.inverse().rotation_matrix(prevTnb);
 
     // calculate the rate of change of velocity (used for launch detect and other functions)

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -102,6 +102,10 @@ public:
     // return NED velocity in m/s
     void getVelNED(Vector3f &vel) const;
 
+    // return estimate of true airspeed vector in body frame in m/s
+    // returns false if estimate is unavailable
+    bool getAirSpdVec(Vector3f &vel) const;
+
     // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
     // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF
     // but will always be kinematically consistent with the z component of the EKF position state