fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 12:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: fix optical flow start logic

Browse Source 
- remove fallthrough that enables flow regardless of success
 - add appropriate start messages for each case
This commit is contained in:
Daniel Agar
2024-06-26 14:53:18 -04:00
parent bf4e564b23
commit bcd666b3f8
1 changed files with 9 additions and 9 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
+9 -9
View File
@@ -126,8 +126,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
void Ekf::startFlowFusion()
{
ECL_INFO("starting optical flow fusion");
if (_height_sensor_ref != HeightSensor::RANGE) {
// If the height is relative to the ground, terrain height cannot be observed.
_hagl_sensor_status.flags.flow = true;
@@ -135,27 +133,29 @@ void Ekf::startFlowFusion()
if (isHorizontalAidingActive()) {
if (fuseOptFlow(_hagl_sensor_status.flags.flow)) {
ECL_INFO("starting optical flow no reset");
ECL_INFO("starting optical flow");
_control_status.flags.opt_flow = true;
} else if (_hagl_sensor_status.flags.flow && !_hagl_sensor_status.flags.range_finder) {
ECL_INFO("starting optical flow, resetting terrain");
resetTerrainToFlow();
} else {
ECL_INFO("optical flow fusion failed to start");
_control_status.flags.opt_flow = false;
_hagl_sensor_status.flags.flow = false;
_control_status.flags.opt_flow = true;
}
} else {
if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) {
ECL_INFO("starting optical flow, resetting");
resetFlowFusion();
_control_status.flags.opt_flow = true;
} else if (_hagl_sensor_status.flags.flow) {
ECL_INFO("starting optical flow, resetting terrain");
resetTerrainToFlow();
_control_status.flags.opt_flow = true;
}
}
_control_status.flags.opt_flow = true; // needs to be here because of isHorizontalAidingActive
_hagl_sensor_status.flags.flow = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE);
}
void Ekf::resetFlowFusion()