dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink : add advanced timesync algorithm

This commit is contained in:
Mohammed Kabir 2018-03-12 01:52:03 -04:00 committed by Beat Küng
parent 6b2daef5ec
commit 39bb65ffd7
8 changed files with 424 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/CMakeLists.txt
View File

@ -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

1
msg/time_offset.msg
View File

@ -1 +0,0 @@
uint64 offset_ns # time offset between companion system and PX4, in nanoseconds

4
msg/timesync_status.msg Normal file
View File

@ -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)

1
src/modules/mavlink/CMakeLists.txt
View File

@ -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

123
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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)
{

21
src/modules/mavlink/mavlink_receiver.h
View File

@ -71,7 +71,6 @@
#include <uORB/topics/debug_vect.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/time_offset.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/follow_target.h>
#include <uORB/topics/landing_target_pose.h>
@ -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;

256
src/modules/mavlink/mavlink_timesync.cpp Normal file
View File

@ -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 <mhkabir98@gmail.com>
*/
#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;
}

150
src/modules/mavlink/mavlink_timesync.h Normal file
View File

@ -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 <mhkabir98@gmail.com>
*/
#pragma once
#include "mavlink_bridge_header.h"
#include <uORB/uORB.h>
#include <uORB/topics/timesync_status.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include <float.h>
#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;
};