fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 07:28:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: optical flow magnitude check compensated

Browse Source 
- additionally don't use flow for reset if magnitude isn't acceptable
This commit is contained in:
Daniel Agar
2024-07-15 09:46:59 -04:00
parent be551097e0
commit a5a67315fd
1 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
+5 -3
View File
@@ -71,8 +71,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
: _params.flow_qual_min_gnd; : _params.flow_qual_min_gnd;
const bool is_quality_good = (flow_sample.quality >= min_quality); const bool is_quality_good = (flow_sample.quality >= min_quality);
const bool is_magnitude_good = flow_sample.flow_rate.isAllFinite()
&& !flow_sample.flow_rate.longerThan(_flow_max_rate);
bool is_tilt_good = true; bool is_tilt_good = true;
@@ -120,6 +118,10 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
const bool is_within_max_sensor_dist = getHagl() <= _flow_max_distance; const bool is_within_max_sensor_dist = getHagl() <= _flow_max_distance;
const bool is_magnitude_good = flow_sample.flow_rate.isAllFinite()
&& !flow_sample.flow_rate.longerThan(_flow_max_rate)
&& !flow_compensated.longerThan(_flow_max_rate);
const bool continuing_conditions_passing = (_params.flow_ctrl == 1) const bool continuing_conditions_passing = (_params.flow_ctrl == 1)
&& _control_status.flags.tilt_align && _control_status.flags.tilt_align
&& is_within_max_sensor_dist; && is_within_max_sensor_dist;
@@ -142,7 +144,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) { if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) {
if (is_flow_required && is_quality_good) { if (is_flow_required && is_quality_good && is_magnitude_good) {
resetFlowFusion(); resetFlowFusion();
if (_control_status.flags.opt_flow_terrain && !isTerrainEstimateValid()) { if (_control_status.flags.opt_flow_terrain && !isTerrainEstimateValid()) {