EKF2/EKF3: bugs that appear after system time wrap

[rmackay9]

While working on improving our handling of external position estimate I found these places in the EKF2/EKF3 where we are doing direct comparisons of the system time (instead of comparison of elapsed system time) meaning the checks probably fail after the system time wraps (which happens after 72day I think):

• EKF2 yaw reset handling in NavEKF2::getLastYawResetAngle, NavEKF2::getLastPosNorthEastReset and NavEKF2::getLastPosDownReset
• EKF3 yaw reset handling in NavEKF3::getLastYawResetAngle, NavEKF3::getLastPosNorthEastReset and NavEKF3::getLastPosDownReset
• NavEKF3's position reset handling after core change in NavEKF3::getLastPosNorthEastReset. directly compares system times (lastCorePosReset_ms and lastPosReset_ms)
• EKF3 logging of odometry (see NavEKF3::Log_Write_BodyOdom here)
• EKF3's getBodyFrameOdomDebug uses a MAX on absolute times which is probably incorrect although this may only cause problems if both wheel odometry and vision-position-delta messages have been passed in since startup and these are probably not compatible.

return MAX(bodyOdmDataDelayed.time_ms,wheelOdmDataDelayed.time_ms);

• EKF3's writeOptFlowMeas is using MAX() on two absolute times which is likely incorrect at least for a moment during the wrap.
• EKF3's readGPSData() function uses a MIN and MAX on absolute times that is likely incorrect.

gpsDataNew.time_ms = MIN(MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms),imuDataDownSampledNew.time_ms);

• EKF3's readBaroData uses a MAX on absolute times which is probably incorrect

baroDataNew.time_ms = MAX(baroDataNew.time_ms,imuDataDelayed.time_ms);

• EKF2's and EKF3's writeExtNavData make the same make as above by using a MAX when setting timeStamp_ms (see here for EKF2 and here for EKF3)
• EKF2's and EKF3's NavEKF3_core::readRangeFinder() methods have incorrect checks like below.

                // don't allow time to go backwards
                if (storedRngMeasTime_ms[sensorIndex][midIndex] > rangeDataNew.time_ms) {
                    rangeDataNew.time_ms = storedRngMeasTime_ms[sensorIndex][midIndex];
                }

System time wrap can be tested in SITL by changing SIM_Aircraft.cpp as shown below so that the time wraps about 30 seconds after starting SITL

time_now_us(4294900000ULL * 1000ULL),

These are fixed:

• EKF2 external navigation position reset timer in NavEKF2_core::writeExtNavData (fix is in this PR)

[tridge]

very nice. We should add a SIM_ param for setting initial time, so we don't need the code change