diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index 09dfb3e6a7..c403973354 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) { + } else if (!_control_status.flags.in_air && is_tilt_good && _control_status.flags.vehicle_at_rest) { if (!is_delta_time_good) { // handle special case of SITL and PX4Flow where dt is forced to @@ -169,10 +169,12 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) if ((_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) // optical flow has been selected by the user && !_control_status.flags.opt_flow // we are not yet using flow data && !inhibit_flow_use - && _range_sensor.isDataHealthy()) { + && !isRecent(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6) + ) { // set the flag and reset the fusion timeout ECL_INFO("starting optical flow fusion"); + updateOptFlow(_aid_src_optical_flow); // if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set if (!isHorizontalAidingActive()) { @@ -193,7 +195,6 @@ 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; diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index 78b579bfd3..993663d6f1 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -140,6 +140,8 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir) // THEN: estimated velocity should match simulated velocity const Vector3f estimated_velocity = _ekf->getVelocity(); + estimated_velocity.print(); + simulated_velocity.print(); EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity)) << "estimated vel = " << estimated_velocity(0) << ", " << estimated_velocity(1);