From 3984ac816afab5aa5e76e2e9541b3bd4efc4836d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 25 Jun 2018 00:34:28 -0400 Subject: [PATCH] EKF only publish wind if it's being estimated --- src/modules/ekf2/ekf2_main.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 34b73d335a7f..002a531358a6 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -1046,6 +1046,9 @@ void Ekf2::run() ekf_solution_status soln_status; soln_status.value = status.solution_status_flags; + filter_control_status_u ekf_control_mask; + ekf_control_mask.value = status.control_mode_flags; + if (updated) { // integrate time to monitor time slippage @@ -1333,7 +1336,9 @@ void Ekf2::run() // reset to prevent data being saved too frequently _total_cal_time_us = 0; } + } + if (ekf_control_mask.flags.wind) { publish_wind_estimate(now); } @@ -1378,9 +1383,7 @@ void Ekf2::run() // set the max allowed yaw innovaton depending on whether we are not aiding navigation using // observations in the NE reference frame. - filter_control_status_u _ekf_control_mask; - _ekf.get_control_mode(&_ekf_control_mask.value); - bool doing_ne_aiding = _ekf_control_mask.flags.gps || _ekf_control_mask.flags.ev_pos; + bool doing_ne_aiding = ekf_control_mask.flags.gps || ekf_control_mask.flags.ev_pos; float yaw_test_limit;