From 694501b782937b5cd809953e89bb82e4248f957d Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Tue, 27 Jun 2023 15:09:20 -0600 Subject: [PATCH] Apply code review changes from @dagar --- src/modules/ekf2/EKF/optflow_fusion.cpp | 7 ++----- src/modules/ekf2/EKF/optical_flow_control.cpp | 4 ++-- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 6ac8ad2e3c..0f179233cb 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -83,13 +83,10 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) Vector2f innov_var; Vector24f H; sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H); - innov_var.copyTo(_aid_src_optical_flow.innovation_variance); + innov_var.copyTo(aid_src.innovation_variance); // run the innovation consistency check and record result - setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f)); - - _innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f); - _innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f); + setEstimatorAidStatusTestRatio(aid_src, math::max(_params.flow_innov_gate, 1.f)); } void Ekf::fuseOptFlow() diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index 2503f70fc5..0df8b140bd 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -59,7 +59,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // Accumulate autopilot gyro data across the same time interval as the flow sensor const Vector3f delta_angle(imu_delayed.delta_ang - (getGyroBias() * imu_delayed.delta_ang_dt)); - if (_delta_time_of < 0.5f) { + if (_delta_time_of < 0.2f) { _imu_del_ang_of += delta_angle; _delta_time_of += imu_delayed.delta_ang_dt; @@ -75,7 +75,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt); const float delta_time_min = fmaxf(0.7f * _delta_time_of, 0.001f); - const float delta_time_max = fminf(1.3f * _delta_time_of, 0.5f); + const float delta_time_max = fminf(1.3f * _delta_time_of, 0.2f); bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min && _flow_sample_delayed.dt <= delta_time_max; if (!is_delta_time_good && (_flow_sample_delayed.dt > FLT_EPSILON)) {