flow: restructure optical flow control logic (#928)

- 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
+1 -1
View File
@@ -79,7 +79,7 @@ void Ekf::resetHorizontalVelocityToOpticalFlow()
// calculate absolute distance from focal point to centre of frame assuming a flat earth
const float range = heightAboveGndEst / _range_sensor.getCosTilt();
if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
if ((range - _params.rng_gnd_clearance) > 0.3f) {
// we should have reliable OF measurements so
// calculate X and Y body relative velocities from OF measurements
Vector3f vel_optflow_body;