From 39bb65ffd7e19e018613d583066903a3883cb18e Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Mon, 12 Mar 2018 01:52:03 -0400 Subject: [PATCH] mavlink : add advanced timesync algorithm --- msg/CMakeLists.txt | 2 +- msg/time_offset.msg | 1 - msg/timesync_status.msg | 4 + src/modules/mavlink/CMakeLists.txt | 1 + src/modules/mavlink/mavlink_receiver.cpp | 123 +---------- src/modules/mavlink/mavlink_receiver.h | 21 +- src/modules/mavlink/mavlink_timesync.cpp | 256 +++++++++++++++++++++++ src/modules/mavlink/mavlink_timesync.h | 150 +++++++++++++ 8 files changed, 424 insertions(+), 134 deletions(-) delete mode 100644 msg/time_offset.msg create mode 100644 msg/timesync_status.msg create mode 100644 src/modules/mavlink/mavlink_timesync.cpp create mode 100644 src/modules/mavlink/mavlink_timesync.h diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index feb8e913dc..1e791b57ab 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -103,7 +103,7 @@ set(msg_files tecs_status.msg telemetry_status.msg test_motor.msg - time_offset.msg + timesync_status.msg transponder_report.msg tune_control.msg uavcan_parameter_request.msg diff --git a/msg/time_offset.msg b/msg/time_offset.msg deleted file mode 100644 index da2d89ab24..0000000000 --- a/msg/time_offset.msg +++ /dev/null @@ -1 +0,0 @@ -uint64 offset_ns # time offset between companion system and PX4, in nanoseconds diff --git a/msg/timesync_status.msg b/msg/timesync_status.msg new file mode 100644 index 0000000000..1ae9adf581 --- /dev/null +++ b/msg/timesync_status.msg @@ -0,0 +1,4 @@ +uint64 remote_timestamp # remote system timestamp (microseconds) +int64 observed_offset # raw time offset directly observed from this timesync packet (microseconds) +int64 estimated_offset # smoothed time offset between companion system and PX4 (microseconds) +uint32 round_trip_time # round trip time of this timesync packet (microseconds) \ No newline at end of file diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 03b3e2a913..ad203e6f3c 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -60,6 +60,7 @@ px4_add_module( mavlink_simple_analyzer.cpp mavlink_stream.cpp mavlink_ulog.cpp + mavlink_timesync.cpp DEPENDS git_mavlink_v2 conversion diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d889c6c610..9b6047f669 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -96,6 +96,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _parameters_manager(parent), _mavlink_ftp(parent), _mavlink_log_handler(parent), + _mavlink_timesync(parent), _status{}, _hil_local_pos{}, _hil_land_detector{}, @@ -128,7 +129,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _manual_pub(nullptr), _obstacle_distance_pub(nullptr), _land_detector_pub(nullptr), - _time_offset_pub(nullptr), _follow_target_pub(nullptr), _landing_target_pose_pub(nullptr), _transponder_report_pub(nullptr), @@ -147,8 +147,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_alt0(0.0f), _hil_local_proj_ref{}, _offboard_control_mode{}, - _time_offset_avg_alpha(0.8), - _time_offset(0), _orb_class_instance(-1), _mom_switch_pos{}, _mom_switch_state(0), @@ -285,14 +283,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_heartbeat(msg); break; - case MAVLINK_MSG_ID_SYSTEM_TIME: - handle_message_system_time(msg); - break; - - case MAVLINK_MSG_ID_TIMESYNC: - handle_message_timesync(msg); - break; - case MAVLINK_MSG_ID_DISTANCE_SENSOR: handle_message_distance_sensor(msg); break; @@ -812,7 +802,7 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg) // Use the component ID to identify the mocap system att_pos_mocap.id = msg->compid; - att_pos_mocap.timestamp = sync_stamp(mocap.time_usec); + att_pos_mocap.timestamp = _mavlink_timesync.sync_stamp(mocap.time_usec); att_pos_mocap.timestamp_received = hrt_absolute_time(); att_pos_mocap.q[0] = mocap.q[0]; @@ -1106,7 +1096,7 @@ MavlinkReceiver::handle_message_attitude_quaternion_cov(mavlink_message_t *msg) struct vehicle_attitude_s vision_attitude = {}; - vision_attitude.timestamp = sync_stamp(att.time_usec); + vision_attitude.timestamp = _mavlink_timesync.sync_stamp(att.time_usec); vision_attitude.q[0] = att.q[0]; vision_attitude.q[1] = att.q[1]; @@ -1132,7 +1122,7 @@ MavlinkReceiver::handle_message_local_position_ned_cov(mavlink_message_t *msg) struct vehicle_local_position_s vision_position = {}; - vision_position.timestamp = sync_stamp(pos.time_usec); + vision_position.timestamp = _mavlink_timesync.sync_stamp(pos.time_usec); vision_position.xy_valid = true; vision_position.z_valid = true; @@ -1192,7 +1182,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) struct vehicle_local_position_s vision_position = {}; - vision_position.timestamp = sync_stamp(pos.usec); + vision_position.timestamp = _mavlink_timesync.sync_stamp(pos.usec); vision_position.x = pos.x; vision_position.y = pos.y; vision_position.z = pos.z; @@ -1204,7 +1194,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) struct vehicle_attitude_s vision_attitude = {}; - vision_attitude.timestamp = sync_stamp(pos.usec); + vision_attitude.timestamp = _mavlink_timesync.sync_stamp(pos.usec); matrix::Quatf q(matrix::Eulerf(pos.roll, pos.pitch, pos.yaw)); q.copyTo(vision_attitude.q); @@ -1793,80 +1783,6 @@ MavlinkReceiver::get_message_interval(int msgId) mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval); } -void -MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) -{ - mavlink_system_time_t time; - mavlink_msg_system_time_decode(msg, &time); - - timespec tv = {}; - px4_clock_gettime(CLOCK_REALTIME, &tv); - - // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 - bool onb_unix_valid = (unsigned long long)tv.tv_sec > PX4_EPOCH_SECS; - bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL; - - if (!onb_unix_valid && ofb_unix_valid) { - tv.tv_sec = time.time_unix_usec / 1000000ULL; - tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; - - if (px4_clock_settime(CLOCK_REALTIME, &tv)) { - PX4_ERR("failed setting clock"); - } - } - -} - -void -MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) -{ - mavlink_timesync_t tsync = {}; - mavlink_msg_timesync_decode(msg, &tsync); - - struct time_offset_s tsync_offset = {}; - - uint64_t now_ns = hrt_absolute_time() * 1000LL ; - - if (tsync.tc1 == 0) { - - mavlink_timesync_t rsync; // return timestamped sync message - - rsync.tc1 = now_ns; - rsync.ts1 = tsync.ts1; - - mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync); - - return; - - } else if (tsync.tc1 > 0) { - - int64_t offset_ns = (int64_t)(tsync.ts1 + now_ns - tsync.tc1 * 2) / 2 ; - int64_t dt = _time_offset - offset_ns; - - if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew - _time_offset = offset_ns; - - // Provide a warning only if not syncing initially - if (_time_offset != 0) { - PX4_ERR("[timesync] Hard setting offset."); - } - - } else { - smooth_time_offset(offset_ns); - } - } - - tsync_offset.offset_ns = _time_offset ; - - if (_time_offset_pub == nullptr) { - _time_offset_pub = orb_advertise(ORB_ID(time_offset), &tsync_offset); - - } else { - orb_publish(ORB_ID(time_offset), _time_offset_pub, &tsync_offset); - } - -} - void MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { @@ -2074,7 +1990,7 @@ void MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg) if (landing_target.position_valid && landing_target.frame == MAV_FRAME_LOCAL_NED) { landing_target_pose_s landing_target_pose = {}; - landing_target_pose.timestamp = sync_stamp(landing_target.time_usec); + landing_target_pose.timestamp = _mavlink_timesync.sync_stamp(landing_target.time_usec); landing_target_pose.abs_pos_valid = true; landing_target_pose.x_abs = landing_target.x; landing_target_pose.y_abs = landing_target.y; @@ -2547,6 +2463,9 @@ MavlinkReceiver::receive_thread(void *arg) /* handle packet with log component */ _mavlink_log_handler.handle_message(&msg); + /* handle packet with timesync component */ + _mavlink_timesync.handle_message(&msg); + /* handle packet with parent object */ _mavlink->handle_message(&msg); } @@ -2587,28 +2506,6 @@ void MavlinkReceiver::print_status() } -uint64_t MavlinkReceiver::sync_stamp(uint64_t usec) -{ - - if (_time_offset != 0) { - return usec + (_time_offset / 1000) ; - - } else { - return hrt_absolute_time(); - } -} - - -void MavlinkReceiver::smooth_time_offset(int64_t offset_ns) -{ - /* The closer alpha is to 1.0, the faster the moving - * average updates in response to new offset samples. - */ - - _time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset; -} - - void *MavlinkReceiver::start_helper(void *context) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 1721c5f549..bb9a05736d 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -71,7 +71,6 @@ #include #include #include -#include #include #include #include @@ -84,8 +83,7 @@ #include "mavlink_parameters.h" #include "mavlink_ftp.h" #include "mavlink_log_handler.h" - -#define PX4_EPOCH_SECS 1234567890ULL +#include "mavlink_timesync.h" class Mavlink; @@ -143,8 +141,6 @@ private: void handle_message_rc_channels_override(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); void handle_message_ping(mavlink_message_t *msg); - void handle_message_system_time(mavlink_message_t *msg); - void handle_message_timesync(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); @@ -178,17 +174,6 @@ private: int set_message_interval(int msgId, float interval, int data_rate = -1); void get_message_interval(int msgId); - /** - * Convert remote timestamp to local hrt time (usec) - * Use timesync if available, monotonic boot time otherwise - */ - uint64_t sync_stamp(uint64_t usec); - - /** - * Exponential moving average filter to smooth time offset - */ - void smooth_time_offset(int64_t offset_ns); - /** * Decode a switch position from a bitfield */ @@ -209,6 +194,7 @@ private: MavlinkParametersManager _parameters_manager; MavlinkFTP _mavlink_ftp; MavlinkLogHandler _mavlink_log_handler; + MavlinkTimesync _mavlink_timesync; mavlink_status_t _status; ///< receiver status, used for mavlink_parse_char() struct vehicle_local_position_s _hil_local_pos; @@ -242,7 +228,6 @@ private: orb_advert_t _manual_pub; orb_advert_t _obstacle_distance_pub; orb_advert_t _land_detector_pub; - orb_advert_t _time_offset_pub; orb_advert_t _follow_target_pub; orb_advert_t _landing_target_pose_pub; orb_advert_t _transponder_report_pub; @@ -262,8 +247,6 @@ private: float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; struct offboard_control_mode_s _offboard_control_mode; - double _time_offset_avg_alpha; - int64_t _time_offset; int _orb_class_instance; static constexpr unsigned MOM_SWITCH_COUNT = 8; diff --git a/src/modules/mavlink/mavlink_timesync.cpp b/src/modules/mavlink/mavlink_timesync.cpp new file mode 100644 index 0000000000..fcda88e803 --- /dev/null +++ b/src/modules/mavlink/mavlink_timesync.cpp @@ -0,0 +1,256 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_timesync.cpp + * Mavlink timesync implementation. + * + * @author Mohammed Kabir + */ + +#include "mavlink_timesync.h" +#include "mavlink_main.h" + +MavlinkTimesync::MavlinkTimesync(Mavlink *mavlink) : + _timesync_status_pub(nullptr), + _sequence(0), + _time_offset(0.0), + _time_skew(0.0), + _filter_alpha(ALPHA_GAIN_INITIAL), + _filter_beta(BETA_GAIN_INITIAL), + _high_deviation_count(0), + _high_rtt_count(0), + _mavlink(mavlink) +{ +} +MavlinkTimesync::~MavlinkTimesync() +{ + if (_timesync_status_pub) { + orb_unadvertise(_timesync_status_pub); + } +} + +void +MavlinkTimesync::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_TIMESYNC: { + + mavlink_timesync_t tsync = {}; + mavlink_msg_timesync_decode(msg, &tsync); + + const uint64_t now = hrt_absolute_time(); + + if (tsync.tc1 == 0) { // Message originating from remote system, timestamp and return it + + mavlink_timesync_t rsync; + + rsync.tc1 = now * 1000ULL; + rsync.ts1 = tsync.ts1; + + mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync); + + return; + + } else if (tsync.tc1 > 0) { // Message originating from this system, compute time offset from it + + // Calculate time offset between this system and the remote system, assuming RTT for + // the timesync packet is roughly equal both ways. + int64_t offset_us = (int64_t)((tsync.ts1 / 1000ULL) + now - (tsync.tc1 / 1000ULL) * 2) / 2 ; + + // Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system + uint64_t rtt_us = now - (tsync.ts1 / 1000ULL); + + // Calculate the difference of this sample from the current estimate + uint64_t deviation = llabs((int64_t)_time_offset - offset_us); + + if (rtt_us < MAX_RTT_SAMPLE) { // Only use samples with low RTT + + if (sync_converged() && (deviation > MAX_DEVIATION_SAMPLE)) { + + // Increment the counter if we have a good estimate and are getting samples far from the estimate + _high_deviation_count++; + + // We reset the filter if we received 5 consecutive samples which violate our present estimate. + // This is most likely due to a time jump on the offboard system. + if (_high_deviation_count > MAX_CONSECUTIVE_HIGH_DEVIATION) { + PX4_ERR("[timesync] Time jump detected. Resetting time synchroniser."); + // Reset the filter + reset_filter(); + } + + } else { + + // Filter gain scheduling + if (!sync_converged()) { + // Interpolate with a sigmoid function + float progress = ((float)_sequence) / CONVERGENCE_WINDOW; + float p = 1.0f - expf(0.5f * (1.0f - 1.0f / (1.0f - progress))); + _filter_alpha = p * (float)ALPHA_GAIN_FINAL + (1.0f - p) * (float)ALPHA_GAIN_INITIAL; + _filter_beta = p * (float)BETA_GAIN_FINAL + (1.0f - p) * (float)BETA_GAIN_INITIAL; + + } else { + _filter_alpha = ALPHA_GAIN_FINAL; + _filter_beta = BETA_GAIN_FINAL; + } + + // Perform filter update + add_sample(offset_us); + + // Increment sequence counter after filter update + _sequence++; + + // Reset high deviation count after filter update + _high_deviation_count = 0; + + // Reset high RTT count after filter update + _high_rtt_count = 0; + } + + } else { + // Increment counter if round trip time is too high for accurate timesync + _high_rtt_count++; + + if (_high_rtt_count > MAX_CONSECUTIVE_HIGH_RTT) { + PX4_WARN("[timesync] RTT too high for timesync: %llu ms", rtt_us / 1000ULL); + // Reset counter to rate-limit warnings + _high_rtt_count = 0; + } + + } + + // Publish status message + struct timesync_status_s tsync_status = {}; + + tsync_status.timestamp = hrt_absolute_time(); + tsync_status.remote_timestamp = tsync.tc1 / 1000ULL; + tsync_status.observed_offset = offset_us; + tsync_status.estimated_offset = (int64_t)_time_offset; + tsync_status.round_trip_time = rtt_us; + + if (_timesync_status_pub == nullptr) { + int instance; + _timesync_status_pub = orb_advertise_multi(ORB_ID(timesync_status), &tsync_status, &instance, ORB_PRIO_DEFAULT); + + } else { + orb_publish(ORB_ID(timesync_status), _timesync_status_pub, &tsync_status); + } + + } + + break; + } + + case MAVLINK_MSG_ID_SYSTEM_TIME: { + + mavlink_system_time_t time; + mavlink_msg_system_time_decode(msg, &time); + + timespec tv = {}; + px4_clock_gettime(CLOCK_REALTIME, &tv); + + // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 + bool onb_unix_valid = (unsigned long long)tv.tv_sec > PX4_EPOCH_SECS; + bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL; + + if (!onb_unix_valid && ofb_unix_valid) { + tv.tv_sec = time.time_unix_usec / 1000000ULL; + tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; + + if (px4_clock_settime(CLOCK_REALTIME, &tv)) { + PX4_ERR("[timesync] Failed setting realtime clock"); + } + } + + break; + } + + default: + break; + } +} + +uint64_t +MavlinkTimesync::sync_stamp(uint64_t usec) +{ + // Only return synchronised stamp if we have converged to a good value + if (sync_converged()) { + return usec + (int64_t)_time_offset; + + } else { + return hrt_absolute_time(); + } +} + +bool +MavlinkTimesync::sync_converged() +{ + return _sequence >= CONVERGENCE_WINDOW; +} + +void +MavlinkTimesync::add_sample(int64_t offset_us) +{ + /* Online exponential smoothing filter. The derivative of the estimate is also + * estimated in order to produce an estimate without steady state lag: + * https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing + */ + + double time_offset_prev = _time_offset; + + if (_sequence == 0) { // First offset sample + _time_offset = offset_us; + + } else { + // Update the clock offset estimate + _time_offset = _filter_alpha * offset_us + (1.0 - _filter_alpha) * (_time_offset + _time_skew); + + // Update the clock skew estimate + _time_skew = _filter_beta * (_time_offset - time_offset_prev) + (1.0 - _filter_beta) * _time_skew; + } + +} + +void +MavlinkTimesync::reset_filter() +{ + // Do a full reset of all statistics and parameters + _sequence = 0; + _time_offset = 0.0; + _time_skew = 0.0; + _filter_alpha = ALPHA_GAIN_INITIAL; + _filter_beta = BETA_GAIN_INITIAL; + _high_deviation_count = 0; + _high_rtt_count = 0; + +} \ No newline at end of file diff --git a/src/modules/mavlink/mavlink_timesync.h b/src/modules/mavlink/mavlink_timesync.h new file mode 100644 index 0000000000..53d1a690ac --- /dev/null +++ b/src/modules/mavlink/mavlink_timesync.h @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_timesync.h + * Mavlink time synchroniser definition. + * + * @author Mohammed Kabir + */ + +#pragma once + +#include "mavlink_bridge_header.h" + +#include +#include + +#include + +#include +#include + +#define PX4_EPOCH_SECS 1234567890ULL + +// Filter gains +// +// Alpha : Used to smooth the overall clock offset estimate. Smaller values will lead +// to a smoother estimate, but track time drift more slowly, introducing a bias +// in the estimate. Larger values will cause low-amplitude oscillations. +// +// Beta : Used to smooth the clock skew estimate. Smaller values will lead to a +// tighter estimation of the skew (derivative), but will negatively affect how fast the +// filter reacts to clock skewing (e.g cause by temperature changes to the oscillator). +// Larger values will cause large-amplitude oscillations. +#define ALPHA_GAIN_INITIAL 0.05 +#define BETA_GAIN_INITIAL 0.05 +#define ALPHA_GAIN_FINAL 0.003 +#define BETA_GAIN_FINAL 0.003 + +// Filter gain scheduling +// +// The filter interpolates between the INITIAL and FINAL gains while the number of +// exhanged timesync packets is less than CONVERGENCE_WINDOW. A lower value will +// allow the timesync to converge faster, but with potentially less accurate initial +// offset and skew estimates. +#define CONVERGENCE_WINDOW 500 + +// Outlier rejection and filter reset +// +// Samples with round-trip time higher than MAX_RTT_SAMPLE are not used to update the filter. +// More than MAX_CONSECUTIVE_HIGH_RTT number of such events in a row will throw a warning +// but not reset the filter. +// Samples whose calculated clock offset is more than MAX_DEVIATION_SAMPLE off from the current +// estimate are not used to update the filter. More than MAX_CONSECUTIVE_HIGH_DEVIATION number +// of such events in a row will reset the filter. This usually happens only due to a time jump +// on the remote system. +// TODO : automatically determine these using ping statistics? +#define MAX_RTT_SAMPLE 10000ULL // 10ms +#define MAX_DEVIATION_SAMPLE 100000ULL // 100ms +#define MAX_CONSECUTIVE_HIGH_RTT 5 +#define MAX_CONSECUTIVE_HIGH_DEVIATION 5 + +class Mavlink; + +class MavlinkTimesync +{ +public: + explicit MavlinkTimesync(Mavlink *mavlink); + ~MavlinkTimesync(); + + void handle_message(const mavlink_message_t *msg); + + /** + * Convert remote timestamp to local hrt time (usec) + * Use synchronised time if available, monotonic boot time otherwise + */ + uint64_t sync_stamp(uint64_t usec); + +private: + + /* do not allow top copying this class */ + MavlinkTimesync(MavlinkTimesync &); + MavlinkTimesync &operator = (const MavlinkTimesync &); + +protected: + + /** + * Online exponential filter to smooth time offset + */ + void add_sample(int64_t offset_us); + + /** + * Return true if the timesync algorithm converged to a good estimate, + * return false otherwise + */ + bool sync_converged(); + + /** + * Reset the exponential filter and its states + */ + void reset_filter(); + + orb_advert_t _timesync_status_pub; + + uint32_t _sequence; + + // Timesync statistics + double _time_offset; + double _time_skew; + + // Filter parameters + double _filter_alpha; + double _filter_beta; + + // Outlier rejection and filter reset + uint32_t _high_deviation_count; + uint32_t _high_rtt_count; + + Mavlink *_mavlink; +};