fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 22:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: resetFlowFusion() pass flowSample by const ref

Browse Source
This commit is contained in:
Daniel Agar
2024-08-26 14:11:30 -04:00
parent 6c24413888
commit f3d58cdf10
2 changed files with 5 additions and 5 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
+4 -4
View File
@@ -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();
@@ -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) {
@@ -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
src/modules/ekf2/EKF/ekf.h
+1 -1
View File
@@ -878,7 +878,7 @@ private:
#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();