diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index a79a5b0a65..45342bc4f9 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -43,6 +43,7 @@ #include "ekf.h" #include #include +#include void Ekf::fuseOptFlow() { @@ -538,7 +539,7 @@ void Ekf::calcOptFlowBias() // if accumulation time differences are not excessive and accumulation time is adequate // compare the optical flow and and navigation rate data and calculate a bias error - if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > 0.01f)) { + if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) { // calculate a reference angular rate Vector3f reference_body_rate; reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of);