diff options
author | M.H.Kabir <mhkabir98@gmail.com> | 2014-12-17 12:12:09 +0530 |
---|---|---|
committer | M.H.Kabir <mhkabir98@gmail.com> | 2014-12-17 12:12:09 +0530 |
commit | 4a42f6ca6a95c9b05ed2c1e66cc251ea0e001c59 (patch) | |
tree | 55feea6346d692ff3bac7cf886a7ed225d99cfb1 /src/modules/mavlink/mavlink_receiver.cpp | |
parent | 0e41624f7902b13bbf830742481174e08e0f97c4 (diff) | |
download | px4-firmware-4a42f6ca6a95c9b05ed2c1e66cc251ea0e001c59.tar.gz px4-firmware-4a42f6ca6a95c9b05ed2c1e66cc251ea0e001c59.tar.bz2 px4-firmware-4a42f6ca6a95c9b05ed2c1e66cc251ea0e001c59.zip |
Restore EMA. Works better for low rates
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 15 |
1 files changed, 14 insertions, 1 deletions
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); |