diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index ee86f47e8c..f38c33aab7 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -322,7 +322,6 @@ private: (ParamExtInt) _flow_qual_min, ///< minimum acceptable quality integer from the flow sensor (ParamExtFloat) _flow_innov_gate, ///< optical flow fusion innovation consistency gate size (STD) - (ParamExtFloat) _flow_rate_max, ///< maximum valid optical flow rate (rad/sec) // sensor positions in body frame (ParamExtFloat) _imu_pos_x, ///< X position of IMU in body frame (m) @@ -470,7 +469,6 @@ Ekf2::Ekf2(): _flow_noise_qual_min(_params->flow_noise_qual_min), _flow_qual_min(_params->flow_qual_min), _flow_innov_gate(_params->flow_innov_gate), - _flow_rate_max(_params->flow_rate_max), _imu_pos_x(_params->imu_pos_body(0)), _imu_pos_y(_params->imu_pos_body(1)), _imu_pos_z(_params->imu_pos_body(2)), @@ -903,6 +901,9 @@ void Ekf2::run() _ekf.setOpticalFlowData(optical_flow.timestamp, &flow); } + // 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); + ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } @@ -931,6 +932,9 @@ void Ekf2::run() _ekf.setRangeData(range_finder.timestamp, range_finder.current_distance); + // Save sensor limits reported by the rangefinder + _ekf.set_rangefinder_limits(range_finder.min_distance, range_finder.max_distance); + ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)range_finder.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } @@ -1127,12 +1131,21 @@ void Ekf2::run() _ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter); // get control limit information - _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.limit_hagl); + _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max); // convert NaN to zero if (!PX4_ISFINITE(lpos.vxy_max)) { lpos.vxy_max = 0.0f; } + if (!PX4_ISFINITE(lpos.vz_max)) { + lpos.vz_max = 0.0f; + } + if (!PX4_ISFINITE(lpos.hagl_min)) { + lpos.hagl_min = 0.0f; + } + if (!PX4_ISFINITE(lpos.hagl_max)) { + lpos.hagl_max = 0.0f; + } // publish vehicle local position data _vehicle_local_position_pub.update(); diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 65a1f16875..542f178922 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -738,17 +738,6 @@ PARAM_DEFINE_INT32(EKF2_OF_QMIN, 1); */ PARAM_DEFINE_FLOAT(EKF2_OF_GATE, 3.0f); -/** - * Optical Flow data will not fused if the magnitude of the flow rate > EKF2_OF_RMAX. - * Control loops will be instructed to limit ground speed such that the flow rate produced by movement over ground is less than 50% of EKF2_OF_RMAX. - * - * @group EKF2 - * @min 1.0 - * @unit rad/s - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_OF_RMAX, 2.5f); - /** * Terrain altitude process noise - accounts for instability in vehicle height estimate *