From 6016b7ec19a57092b732b5e9d23780623b521925 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 26 Jan 2023 12:35:45 -0500 Subject: [PATCH] hackity hack hack --- src/modules/ekf2/EKF/common.h | 1 + src/modules/ekf2/EKF/ekf_helper.cpp | 2 ++ src/modules/ekf2/EKF/optflow_fusion.cpp | 16 ++----------- src/modules/ekf2/EKF/optical_flow_control.cpp | 23 +++++++++---------- src/modules/ekf2/EKF2.cpp | 1 + 5 files changed, 17 insertions(+), 26 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index c75ed2dae7..9b4a854c67 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -236,6 +236,7 @@ struct flowSample { Vector3f gyro_xyz{}; ///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive float dt{}; ///< amount of integration time (sec) uint8_t quality{}; ///< quality indicator between 0 and 255 + float ground_distance_m{NAN}; ///< optical range finder measurement (m) if available }; struct extVisionSample { diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index ffca0b2a93..54e0efd540 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -200,6 +200,8 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z); _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z); + _terrain_vpos -= delta_z; + // Reset the timout timer _time_last_hgt_fuse = _time_delayed_us; } diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 198d6a344c..7ae2927e19 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -77,24 +77,12 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed); aid_src.observation_variance[0] = R_LOS; aid_src.observation_variance[1] = R_LOS; - - - // compute innovation variance to update test ratios - const Vector24f state_vector = getStateAtFusionHorizonAsVector(); - 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); - - // 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); } void Ekf::fuseOptFlow() { + _aid_src_optical_flow.fusion_enabled = true; + const float R_LOS = _aid_src_optical_flow.observation_variance[0]; // calculate the height above the ground of the optical flow camera. Since earth frame is NED diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index 4984ae0c85..09dfb3e6a7 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -111,7 +111,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // compensate for body motion to give a LOS rate _flow_compensated_XY_rad = _flow_sample_delayed.flow_xy_rad - _flow_sample_delayed.gyro_xyz.xy(); - } else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) { + } else if (!_control_status.flags.in_air) { if (!is_delta_time_good) { // handle special case of SITL and PX4Flow where dt is forced to @@ -129,8 +129,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) _flow_compensated_XY_rad.setZero(); } - updateOptFlow(_aid_src_optical_flow); - } else { _flow_compensated_XY_rad.setZero(); } @@ -173,13 +171,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) && !inhibit_flow_use && _range_sensor.isDataHealthy()) { - if (isHorizontalAidingActive() - && ((_aid_src_optical_flow.test_ratio[0] > 0.1f) || (_aid_src_optical_flow.test_ratio[1] > 0.1f)) - ) { - // if horizontal aiding already active don't allow optical flow to start unless the test ratio is good - return; - } - // set the flag and reset the fusion timeout ECL_INFO("starting optical flow fusion"); @@ -202,7 +193,16 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) } } + updateOptFlow(_aid_src_optical_flow); + _aid_src_optical_flow.fusion_enabled = true; + _aid_src_optical_flow.test_ratio[0] = 0.f; + _aid_src_optical_flow.test_ratio[1] = 0.f; + _aid_src_optical_flow.innovation_rejected = false; _aid_src_optical_flow.time_last_fuse = _time_delayed_us; + + _innov_check_fail_status.flags.reject_optflow_X = false; + _innov_check_fail_status.flags.reject_optflow_Y = false; + _control_status.flags.opt_flow = true; return; @@ -214,8 +214,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently // but use a relaxed time criteria to enable it to coast through bad range finder data if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) { - - _aid_src_optical_flow.fusion_enabled = true; + updateOptFlow(_aid_src_optical_flow); fuseOptFlow(); _last_known_pos.xy() = _state.pos.xy(); } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index d7d977f97c..98d34b1816 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1972,6 +1972,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) .gyro_xyz = Vector3f{-optical_flow.delta_angle[0], -optical_flow.delta_angle[1], -optical_flow.delta_angle[2]}, .dt = 1e-6f * (float)optical_flow.integration_timespan_us, .quality = optical_flow.quality, + .ground_distance_m = optical_flow.distance_m, }; if (Vector2f(optical_flow.pixel_flow).isAllFinite() && flow.dt < 1) {