55 case MAVLINK_MSG_ID_TIMESYNC: {
57 mavlink_timesync_t tsync = {};
58 mavlink_msg_timesync_decode(msg, &tsync);
64 mavlink_timesync_t rsync;
66 rsync.tc1 = now * 1000ULL;
67 rsync.ts1 = tsync.ts1;
73 }
else if (tsync.tc1 > 0) {
77 int64_t offset_us = (int64_t)((tsync.ts1 / 1000ULL) + now - (tsync.tc1 / 1000ULL) * 2) / 2 ;
80 uint64_t rtt_us = now - (tsync.ts1 / 1000ULL);
83 uint64_t deviation = llabs((int64_t)
_time_offset - offset_us);
95 PX4_ERR(
"[timesync] Time jump detected. Resetting time synchroniser.");
106 double p = 1.0 - exp(0.5 * (1.0 - 1.0 / (1.0 - progress)));
133 PX4_WARN(
"[timesync] RTT too high for timesync: %llu ms", rtt_us / 1000ULL);
144 tsync_status.remote_timestamp = tsync.tc1 / 1000ULL;
145 tsync_status.observed_offset = offset_us;
147 tsync_status.round_trip_time = rtt_us;
155 case MAVLINK_MSG_ID_SYSTEM_TIME: {
157 mavlink_system_time_t time;
158 mavlink_msg_system_time_decode(msg, &time);
161 px4_clock_gettime(CLOCK_REALTIME, &tv);
164 bool onb_unix_valid = (
unsigned long long)tv.tv_sec >
PX4_EPOCH_SECS;
165 bool ofb_unix_valid = time.time_unix_usec >
PX4_EPOCH_SECS * 1000ULL;
167 if (!onb_unix_valid && ofb_unix_valid) {
168 tv.tv_sec = time.time_unix_usec / 1000000ULL;
169 tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL;
171 if (px4_clock_settime(CLOCK_REALTIME, &tv)) {
172 PX4_ERR(
"[timesync] Failed setting realtime clock");
static constexpr uint32_t MAX_CONSECUTIVE_HIGH_DEVIATION
bool sync_converged()
Return true if the timesync algorithm converged to a good estimate, return false otherwise.
MAVLink 2.0 protocol interface definition.
void add_sample(int64_t offset_us)
Online exponential filter to smooth time offset.
void handle_message(const mavlink_message_t *msg)
static constexpr double BETA_GAIN_FINAL
void reset_filter()
Reset the exponential filter and its states.
uORB::PublicationMulti< timesync_status_s > _timesync_status_pub
static constexpr double BETA_GAIN_INITIAL
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Mavlink time synchroniser definition.
static constexpr uint32_t CONVERGENCE_WINDOW
static constexpr double ALPHA_GAIN_FINAL
mavlink_channel_t get_channel() const
uint64_t sync_stamp(uint64_t usec)
Convert remote timestamp to local hrt time (usec) Use synchronised time if available, monotonic boot time otherwise.
static constexpr uint64_t MAX_RTT_SAMPLE
static constexpr uint64_t MAX_DEVIATION_SAMPLE
static constexpr uint32_t MAX_CONSECUTIVE_HIGH_RTT
bool publish(const T &data)
Publish the struct.
static constexpr double ALPHA_GAIN_INITIAL
uint32_t _high_deviation_count
MavlinkTimesync(Mavlink *mavlink)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
static constexpr time_t PX4_EPOCH_SECS