diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index aec4e40455..f52cb8e15d 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -224,6 +224,12 @@ void Ekf::get_wind_velocity(float *wind) wind[1] = _state.wind_vel(1); } +void Ekf::get_wind_velocity_var(float *wind_var) +{ + wind_var[0] = P[22][22]; + wind_var[1] = P[23][23]; +} + 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))); diff --git a/EKF/ekf.h b/EKF/ekf.h index 802e7ada52..bfdc77ba3b 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 wind velocity var + void get_wind_velocity_var(float *wind_var); + // get the true airspeed in m/s void get_true_airspeed(float *tas); diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 3d49c62f3e..a386232720 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_wind_velocity_var(float *wind_var) = 0; + virtual void get_true_airspeed(float *tas) = 0; virtual void get_covariances(float *covariances) = 0;