diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index c88712b6d6..fbdd4417dd 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -202,3 +202,9 @@ void Ekf::fuseAirspeed() } } } + +void Ekf::get_wind_velocity(float *wind) +{ + wind[0] = _state.wind_vel(0); + wind[1] = _state.wind_vel(1); +} diff --git a/EKF/ekf.h b/EKF/ekf.h index 3412d1cf4f..167ed9cffa 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -97,6 +97,9 @@ public: // get the state vector at the delayed time horizon void get_state_delayed(float *state); + // get the wind velocity in m/s + void get_wind_velocity(float *wind); + // 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 44880a9213..dfe192da83 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -84,6 +84,8 @@ public: virtual void get_state_delayed(float *state) = 0; + virtual void get_wind_velocity(float *wind) = 0; + virtual void get_covariances(float *covariances) = 0; // get the ekf WGS-84 origin position and height and the system time it was last set