Skip to content

Commit

Permalink
ekf2-flow: only allow flow when in range
Browse files Browse the repository at this point in the history
Also, as the flow makes the link between range and horizontal velocity,
only allow it to start if at least one of the two is known. Otherwise
the EKF will struggle to estimate both values at the same time.
  • Loading branch information
bresch committed Jul 15, 2024
1 parent be55109 commit 24d19fd
Showing 1 changed file with 3 additions and 2 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -118,16 +118,17 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));

const bool is_within_max_sensor_dist = getHagl() <= _flow_max_distance;
const bool is_within_sensor_dist = (getHagl() >= _flow_min_distance) && (getHagl() <= _flow_max_distance);

const bool continuing_conditions_passing = (_params.flow_ctrl == 1)
&& _control_status.flags.tilt_align
&& is_within_max_sensor_dist;
&& is_within_sensor_dist;

const bool starting_conditions_passing = continuing_conditions_passing
&& is_quality_good
&& is_magnitude_good
&& is_tilt_good
&& (isTerrainEstimateValid() || isHorizontalAidingActive())
&& isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching

// If the height is relative to the ground, terrain height cannot be observed.
Expand Down

0 comments on commit 24d19fd

Please sign in to comment.