diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 3b9b3fba15..6ac8ad2e3c 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -90,8 +90,6 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) _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); - - // PX4_WARN("updateOptFlow: %f,%f %f,%f\n", (double)aid_src.innovation[0], (double)aid_src.innovation[1], (double)aid_src.innovation_variance[0], (double)aid_src.innovation_variance[1]); } void Ekf::fuseOptFlow() diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index a4f93ac38d..74e5593449 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -59,7 +59,6 @@ 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)); - // increase limit from 0.1f to 0.5f if (_delta_time_of < 0.5f) { _imu_del_ang_of += delta_angle; _delta_time_of += imu_delayed.delta_ang_dt; @@ -76,7 +75,6 @@ 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); - // increase hard max to 0.5f from 0.2f const float delta_time_max = fminf(1.3f * _delta_time_of, 0.5f); bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min && _flow_sample_delayed.dt <= delta_time_max; @@ -206,15 +204,10 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) if (_time_delayed_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) { // 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 - - // give much larger timeout (10x) for hagl fuse if (isRecent(_time_last_hagl_fuse, (uint64_t)100e6)) { fuseOptFlow(); _last_known_pos.xy() = _state.pos.xy(); } - // else { - // PX4_WARN("terrain estimator sad %ld", _time_last_hagl_fuse); - // } _flow_data_ready = false; }