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

ekf2: flow fusion start require valid fusion

Browse Source
This commit is contained in:
Daniel Agar 2024-06-26 14:48:03 -04:00
parent 1df8f3f9d2
commit ced609daa8
3 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
View File

@ -134,9 +134,8 @@ void Ekf::startFlowFusion()
}
if (isHorizontalAidingActive()) {
if (!_aid_src_optical_flow.innovation_rejected) {
if (fuseOptFlow(_hagl_sensor_status.flags.flow)) {
ECL_INFO("starting optical flow no reset");
fuseOptFlow(_hagl_sensor_status.flags.flow);
} else if (_hagl_sensor_status.flags.flow && !_hagl_sensor_status.flags.range_finder) {
resetTerrainToFlow();

12
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
View File

@ -79,7 +79,7 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
math::max(_params.flow_innov_gate, 1.f)); // innovation gate
}
void Ekf::fuseOptFlow(const bool update_terrain)
bool Ekf::fuseOptFlow(const bool update_terrain)
{
const float R_LOS = _aid_src_optical_flow.observation_variance[0];
@ -98,7 +98,7 @@ void Ekf::fuseOptFlow(const bool update_terrain)
// we need to reinitialise the covariance matrix and abort this fusion step
ECL_ERR("Opt flow error - covariance reset");
initialiseCovariance();
return;
return false;
}
_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
@ -106,7 +106,7 @@ void Ekf::fuseOptFlow(const bool update_terrain)
// if either axis fails we abort the fusion
if (_aid_src_optical_flow.innovation_rejected) {
return;
return false;
}
bool fused[2] {false, false};
@ -129,7 +129,7 @@ void Ekf::fuseOptFlow(const bool update_terrain)
// we need to reinitialise the covariance matrix and abort this fusion step
ECL_ERR("Opt flow error - covariance reset");
initialiseCovariance();
return;
return false;
}
}
@ -150,7 +150,11 @@ void Ekf::fuseOptFlow(const bool update_terrain)
if (fused[0] && fused[1]) {
_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
_aid_src_optical_flow.fused = true;
return true;
}
return false;
}
float Ekf::predictFlowRange()

2
src/modules/ekf2/EKF/ekf.h
View File

@ -850,7 +850,7 @@ private:
// fuse optical flow line of sight rate measurements
void updateOptFlow(estimator_aid_source2d_s &aid_src);
void fuseOptFlow(bool update_terrain);
bool fuseOptFlow(bool update_terrain);
float predictFlowRange();
Vector2f predictFlowVelBody();
#endif // CONFIG_EKF2_OPTICAL_FLOW