diff options
author | M.H.Kabir <mhkabir98@gmail.com> | 2014-12-16 20:39:28 +0530 |
---|---|---|
committer | M.H.Kabir <mhkabir98@gmail.com> | 2014-12-16 20:39:28 +0530 |
commit | 0e41624f7902b13bbf830742481174e08e0f97c4 (patch) | |
tree | 39c92bb28f754604287c796db14216672f5caadb | |
parent | 5c6155c94d3a27cdc426f7d3e18a679f0bc589e7 (diff) | |
download | px4-firmware-0e41624f7902b13bbf830742481174e08e0f97c4.tar.gz px4-firmware-0e41624f7902b13bbf830742481174e08e0f97c4.tar.bz2 px4-firmware-0e41624f7902b13bbf830742481174e08e0f97c4.zip |
removed exponential filter. simple weighted average works fine.
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 19 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 6 |
2 files changed, 4 insertions, 21 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 88d12e775..cd385ffac 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -122,7 +122,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_proj_inited(0), _hil_local_alt0(0.0f), _hil_local_proj_ref{}, - _time_offset_avg_alpha(0.75), _time_offset(0) { @@ -971,15 +970,15 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } else if (tsync.tc1 > 0) { - int64_t offset_ns = ((tsync.ts1 + now_ns) - (tsync.tc1 * 2)) / 2; + int64_t offset_ns = (9*_time_offset + (tsync.ts1 + now_ns - tsync.tc1*2)/2 )/10; // average offset int64_t dt = _time_offset - offset_ns; if (dt > 10000000 || dt < -1000000) { // 10 millisecond skew - _time_offset = offset_ns; - warnx("[timesync] Timesync offset is off. Hard-setting offset"); + _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; + warnx("[timesync] Companion clock offset is skewed. Hard-setting offset"); } else { - average_time_offset(offset_ns); + _time_offset = offset_ns; } } @@ -1457,16 +1456,6 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec) return usec - (_time_offset / 1000) ; } -void MavlinkReceiver::average_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 71273786d..7c79f661b 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -137,11 +137,6 @@ private: */ uint64_t to_hrt(uint64_t nsec); - /** - * Exponential moving average filter to smooth time offset - */ - void average_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; @@ -176,7 +171,6 @@ 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 */ |