diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 3adae54bb8..0d720f7589 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -225,6 +225,12 @@ void Ekf::get_wind_velocity(float *wind) wind[1] = _state.wind_vel(1); } +void Ekf::get_true_airspeed(float *tas) +{ + float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2))); + memcpy(tas, &tempvar, sizeof(float)); +} + /* * Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip */ diff --git a/EKF/ekf.h b/EKF/ekf.h index 91c7e191e0..38329d56b3 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -112,6 +112,9 @@ public: // get the wind velocity in m/s void get_wind_velocity(float *wind); + // get the true airspeed in m/s + void get_true_airspeed(float *tas); + // get the diagonal elements of the covariance matrix void get_covariances(float *covariances); diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index cec722fb2a..9d652a4c14 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -93,6 +93,8 @@ public: virtual void get_wind_velocity(float *wind) = 0; + virtual void get_true_airspeed(float *tas) = 0; + virtual void get_covariances(float *covariances) = 0; // gets the variances for the NED velocity states