When Copter arms, the AHRS/EKF may not be run for a few hundred msec depending on conditions. This can cause the arming check to fail the optical flow sensor and place the EKF in a constant position mode.
This additional explicit setting of the constPosMode and constVelMode reduces the likelihood of logic errors being introduced in the future as it places the intended setting of these parameters at arming in the one place. the constVelmode and constPosMode only have one set of conditions each that can trigger these modes in flight, so if these modes are true after arming it will be clear that it was the in-flight condition that triggered.
This ensures the position and velocity measurement status will be set as timed out immediately after use of those measurements is inhibited. This will improve the timeliness of filter status reporting.
It does not make sense to relax the limits on vehicle speed and nav gains just because we have received some invalid flow data. This could make the situation worse if the invalid data was being caused by too much speed.
If we are relying on flow data the vehicle limits should always be applied.
When we are not using GPS measurements, we should not be allowing the GPS glitch logic to reset position states as this can interfere with operation of non GPS modes.
When PV aiding is disabled, then the timeout time reference should not be reset becasue we want the position measurement timeout status to remain true the whole time the measurement is not being used.
Synchronise with covariance prediction to improve numerical stability and accuracy of angle corrections. The 'noise' this produces in the position and velocity estimate is irrelevantbecause these are not used by the control loops during this mode of operation (they are nominally zero anyway).
Prevents possible loss of attitude reference for flights without optical flow and GPS.
The optical flow measurement timeout can reset the velocity states which decouples the position states from IMU errors and therefore significantly reduces the amount of attitude error correction.
This enables the filter to report the last known position after disarm.
The LLH location of the filters NED origin is published and should be logged every time the vehicle is armed to assist with post-flight trajectory reconstruction.
The LLH location of the filters NED origin can be set externally whilst the vehicle is disarmed.
The two state auxiliary EKF has been replaced with a single state filter that only estimates terrain offset. The new filter fuses a optical flow line of sight rate scalar (length of the optical flow LOS rate vector) which provides a terrain offset estimate that is less affected by yaw errors.
Estimation of focal length scale factor error in flight wasn't accurate enough and will be replaced with a pre-flight intrinsic sensor calibration procedure as the scale factor error does not change over time provided the lens assembly is not adjusted.
AP_NavEKF: Remove unwanted printf
Paul Riseborough
Andrew Tridgell
Randy Mackay