While a yaw test ration > 1 rejects the mag data in air, the fusion is
forced with a limited innovation on ground to fix gyro bias and local
magnetic effects problems pre-takeoff. However, the same logic is
applied after landing and immediately after the land detector detects
landing. In some cases, this can disturb the other EKF states which can
lead to a state change of the land detector.
To avoid such a loop, the start of this forced yaw fusion is delayed by
5 seconds after landing.
These changes fix a bug that allowed IMU gyro errors that cause large tilt errors prior to start of aiding not resulting in large innovation test ratios.
The failsafe from rng height to baro height should occur after a reset
to baro triggered by the controlHeightSensorTimeouts function and not in
the fusion selector.
Until now, the EKF was fusing baro measurements between rng updates if
the range finder ODR was slower than the EKF update rate. This is not
the case anymore as the height reset occurs after 5 seconds.
The unit test has been extended to show this behavior.
- 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 fixes the cases where the yaw message from the GNSS receiver would
take more time than the vel/pos. The estimator should wait and not
immediately fall back to an other aiding source after 5sec.
If it never comes, it will never fall back, but this is ok since the
user wants to fly with GPS aiding an not with something else.
If the user selects GPS yaw fusion but that there is no GPS yaw data in
the GPS message or if the fusion is rejected for some time, the GPS yaw
data is declared faulty and the fusion is stopped to allow an other
source of yaw aiding to start.
* Support vision velocity expressed in body frame
* Use switch statement for vision velocity frame
* Robustify vision velocity frame test
* Increase lower bound on vision velocity noise to 0.05 m/s
