Skip to content

Commit

Permalink
ekf2: resetFlowFusion() pass flowSample by const ref
Browse files Browse the repository at this point in the history
  • Loading branch information
dagar committed Aug 27, 2024
1 parent 6c24413 commit f3d58cd
Show file tree
Hide file tree
Showing 2 changed files with 5 additions and 5 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -169,7 +169,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
if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) {
if (is_flow_required && is_quality_good && is_magnitude_good) {
resetFlowFusion();
resetFlowFusion(flow_sample);

if (_control_status.flags.opt_flow_terrain && !isTerrainEstimateValid()) {
resetTerrainToFlow();
Expand Down Expand Up @@ -203,7 +203,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
} else {
if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) {
ECL_INFO("starting optical flow, resetting");
resetFlowFusion();
resetFlowFusion(flow_sample);
_control_status.flags.opt_flow = true;

} else if (_control_status.flags.opt_flow_terrain) {
Expand All @@ -222,12 +222,12 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
}
}

void Ekf::resetFlowFusion()
void Ekf::resetFlowFusion(const flowSample &flow_sample)
{
ECL_INFO("reset velocity to flow");
_information_events.flags.reset_vel_to_flow = true;

const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(_flow_sample_delayed);
const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(flow_sample);
resetHorizontalVelocityTo(getFilteredFlowVelNE(), flow_vel_var);

// reset position, estimate is relative to initial position in this mode, so we start with zero error
Expand Down
2 changes: 1 addition & 1 deletion src/modules/ekf2/EKF/ekf.h
Original file line number Diff line number Diff line change
Expand Up @@ -878,7 +878,7 @@ class Ekf final : public EstimatorInterface
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
// control fusion of optical flow observations
void controlOpticalFlowFusion(const imuSample &imu_delayed);
void resetFlowFusion();
void resetFlowFusion(const flowSample &flow_sample);
void stopFlowFusion();

void updateOnGroundMotionForOpticalFlowChecks();
Expand Down

0 comments on commit f3d58cd

Please sign in to comment.