diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index f38c33aab7..e54c9d4f55 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -902,7 +902,8 @@ void Ekf2::run() } // Save sensor limits reported by the optical flow sensor - _ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance, optical_flow.max_ground_distance); + _ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance, + optical_flow.max_ground_distance); ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); @@ -1133,18 +1134,21 @@ void Ekf2::run() // get control limit information _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max); - // convert NaN to zero + // convert NaN to INFINITY if (!PX4_ISFINITE(lpos.vxy_max)) { - lpos.vxy_max = 0.0f; + lpos.vxy_max = INFINITY; } + if (!PX4_ISFINITE(lpos.vz_max)) { - lpos.vz_max = 0.0f; + lpos.vz_max = INFINITY; } + if (!PX4_ISFINITE(lpos.hagl_min)) { - lpos.hagl_min = 0.0f; + lpos.hagl_min = INFINITY; } + if (!PX4_ISFINITE(lpos.hagl_max)) { - lpos.hagl_max = 0.0f; + lpos.hagl_max = INFINITY; } // publish vehicle local position data