From c20e4e442156449da9c3fbec2a6096a8ad6be280 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 21 Feb 2023 11:08:03 -0500 Subject: [PATCH] ekf2: move stopFlowFusion() to optical_flow_control.cpp --- src/modules/ekf2/EKF/ekf_helper.cpp | 10 ---------- src/modules/ekf2/EKF/optical_flow_control.cpp | 10 ++++++++++ 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index f8d356da80..c3c78453df 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1188,16 +1188,6 @@ void Ekf::stopAuxVelFusion() resetEstimatorAidStatus(_aid_src_aux_vel); } -void Ekf::stopFlowFusion() -{ - if (_control_status.flags.opt_flow) { - ECL_INFO("stopping optical flow fusion"); - _control_status.flags.opt_flow = false; - - resetEstimatorAidStatus(_aid_src_optical_flow); - } -} - void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) { // save a copy of the quaternion state for later use in calculating the amount of reset change diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index 2d08ebd1c5..d143cb4684 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -258,3 +258,13 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks() _time_bad_motion_us = 0; _time_good_motion_us = _time_delayed_us; } + +void Ekf::stopFlowFusion() +{ + if (_control_status.flags.opt_flow) { + ECL_INFO("stopping optical flow fusion"); + _control_status.flags.opt_flow = false; + + resetEstimatorAidStatus(_aid_src_optical_flow); + } +}