From 4a42f6ca6a95c9b05ed2c1e66cc251ea0e001c59 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Wed, 17 Dec 2014 12:12:09 +0530 Subject: Restore EMA. Works better for low rates --- src/modules/mavlink/mavlink_receiver.cpp | 15 ++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 7 ++++++- 2 files changed, 20 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cd385ffac..5aa41e5d8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -122,6 +122,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_proj_inited(0), _hil_local_alt0(0.0f), _hil_local_proj_ref{}, + _time_offset_avg_alpha(0.6), _time_offset(0) { @@ -978,7 +979,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) warnx("[timesync] Companion clock offset is skewed. Hard-setting offset"); } else { - _time_offset = offset_ns; + smooth_time_offset(offset_ns); } } @@ -1456,6 +1457,18 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec) return usec - (_time_offset / 1000) ; } + +void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns) +{ + /* alpha = 0.75 fixed for now. 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) { MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 7c79f661b..a677e75cd 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -136,7 +136,11 @@ private: * Convert remote nsec timestamp to local hrt time (usec) */ uint64_t to_hrt(uint64_t nsec); - + /** + * Exponential moving average filter to smooth time offset + */ + void smooth_time_offset(uint64_t offset_ns); + mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; struct vehicle_control_mode_s _control_mode; @@ -171,6 +175,7 @@ private: bool _hil_local_proj_inited; float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; + double _time_offset_avg_alpha; uint64_t _time_offset; /* do not allow copying this class */ -- cgit v1.2.3