diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index dbb342518627aec9fab71d109b6b0e99d4c303e7..1ebc9127a571133c1f3684a37cac59e78a269157 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -104,7 +104,9 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), nedPosGlobalOffset(0,0,0), nedAttGlobalOffset(0,0,0), connectionLost(false), - lastVoltageWarning(0) + lastVoltageWarning(0), + lastNonNullTime(0), + onboardTimeOffsetInvalidCount(0) { for (unsigned int i = 0; i<255;++i) @@ -1533,6 +1535,26 @@ quint64 UAS::getUnixTimeFromMs(quint64 time) */ quint64 UAS::getUnixTime(quint64 time) { + // Check if the offset estimation likely went wrong + // and we're talking to a new instance / the system + // has rebooted. Only reset if this is consistent. + if (time != 0 && lastNonNullTime > time) + { + onboardTimeOffsetInvalidCount++; + } + else if (lastNonNullTime < time) + { + onboardTimeOffsetInvalidCount = 0; + } + + // Reset onboard time offset estimation, since it seems to be really off + if (onboardTimeOffsetInvalidCount > 20) + { + onboardTimeOffset = 0; + onboardTimeOffsetInvalidCount = 0; + } + + quint64 ret = 0; if (attitudeStamped) { diff --git a/src/uas/UAS.h b/src/uas/UAS.h index f5fe0df4e87bfee134306df99769aa794f4cebcc..a2769aad92f63f7095410f766f35ad8a7ebffa36 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -674,6 +674,8 @@ protected: bool connectionLost; ///< Flag indicates a timed out connection quint64 connectionLossTime; ///< Time the connection was interrupted quint64 lastVoltageWarning; ///< Time at which the last voltage warning occured + quint64 lastNonNullTime; ///< The last timestamp from the MAV that was not null + unsigned int onboardTimeOffsetInvalidCount; ///< Count when the offboard time offset estimation seemed wrong protected slots: /** @brief Write settings to disk */