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

flow: restructure optical flow control logic (#928)

Browse Source 
- extract motion checks performed on ground
- move all non-timing check to controlOpticalFlowFusion. This simplifies and standardirzes the setOpticalFlowData function. Furthermore, in some cases (SITL, PX4Flow), the dt is forced to 0 when the quality is 0 (which is usual on ground) so the ekf needs to ignore those values on ground to initialize properly the flow fusion.
- add filter convergence test
- move check for dt time max in setOpticalFlowData
- in the simulator, do not update dt as this is the sensor integration
period.
This commit is contained in:
Mathieu Bresciani
2020-12-09 11:33:00 +01:00
committed by GitHub
parent 38358002bb
commit 8f3df7a97b
9 changed files with 193 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/optflow_fusion.cpp
+5 -8
View File
@@ -108,13 +108,8 @@ void Ekf::fuseOptFlow()
_flow_vel_body(1) = opt_flow_rate(0) * range;
_flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));
if (opt_flow_rate.norm() < _flow_max_rate) {
_flow_innov(0) = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
_flow_innov(1) = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis
} else {
return;
}
_flow_innov(0) = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
_flow_innov(1) = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis
// The derivation allows for an arbitrary body to flow sensor frame rotation which is
// currently not supported by the EKF, so assume sensor frame is aligned with the
@@ -350,7 +345,9 @@ bool Ekf::calcOptFlowBodyRateComp()
// if accumulation time differences are not excessive and accumulation time is adequate
// compare the optical flow and and navigation rate data and calculate a bias error
if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) {
if ((_delta_time_of > FLT_EPSILON)
&& (_flow_sample_delayed.dt > FLT_EPSILON)
&& (fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f)) {
const Vector3f reference_body_rate(_imu_del_ang_of * (1.0f / _delta_time_of));