From 8200e4b21802a2374335037506a9644b4dc61e19 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Mon, 2 Oct 2017 14:47:56 +0200 Subject: [PATCH] EKF: added get_wind_velocity_var function Signed-off-by: CarlOlsson --- EKF/airspeed_fusion.cpp | 6 ++++++ EKF/ekf.h | 3 +++ EKF/estimator_interface.h | 2 ++ 3 files changed, 11 insertions(+) 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 b0dc5501e7..133322e974 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;