fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 11:31: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-07-09 10:10:01 -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() void Ekf::startFlowFusion()
{ {
ECL_INFO("starting optical flow fusion");
if (_height_sensor_ref != HeightSensor::RANGE) { if (_height_sensor_ref != HeightSensor::RANGE) {
// If the height is relative to the ground, terrain height cannot be observed. // If the height is relative to the ground, terrain height cannot be observed.
_hagl_sensor_status.flags.flow = true; _hagl_sensor_status.flags.flow = true;
@@ -135,27 +133,29 @@ void Ekf::startFlowFusion()
if (isHorizontalAidingActive()) { if (isHorizontalAidingActive()) {
if (fuseOptFlow(_hagl_sensor_status.flags.flow)) { 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) { } else if (_hagl_sensor_status.flags.flow && !_hagl_sensor_status.flags.range_finder) {
ECL_INFO("starting optical flow, resetting terrain");
resetTerrainToFlow(); resetTerrainToFlow();
_control_status.flags.opt_flow = true;
} else {
ECL_INFO("optical flow fusion failed to start");
_control_status.flags.opt_flow = false;
_hagl_sensor_status.flags.flow = false;
} }
} else { } else {
if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) { if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) {
ECL_INFO("starting optical flow, resetting");
resetFlowFusion(); resetFlowFusion();
_control_status.flags.opt_flow = true;
} else if (_hagl_sensor_status.flags.flow) { } else if (_hagl_sensor_status.flags.flow) {
ECL_INFO("starting optical flow, resetting terrain");
resetTerrainToFlow(); 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() void Ekf::resetFlowFusion()