diff options
| author | M.H.Kabir <mhkabir98@gmail.com> | 2014-12-16 13:43:48 +0530 |
|---|---|---|
| committer | M.H.Kabir <mhkabir98@gmail.com> | 2014-12-16 13:43:48 +0530 |
| commit | 8c0d7047b209c23cb14b3a09bf20599af0f8cc6e (patch) | |
| tree | dfa467aca94ec0aa93982a764edbb1d89aee35ac /src/modules/mavlink | |
| parent | e1bdc4a0fbb5e89dc5f3911dbf9a9a21ed5b459b (diff) | |
| download | px4-firmware-8c0d7047b209c23cb14b3a09bf20599af0f8cc6e.tar.gz px4-firmware-8c0d7047b209c23cb14b3a09bf20599af0f8cc6e.tar.bz2 px4-firmware-8c0d7047b209c23cb14b3a09bf20599af0f8cc6e.zip | |
Working now.
Diffstat (limited to 'src/modules/mavlink')
| -rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 2 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 16 |
2 files changed, 10 insertions, 8 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 85643270f..dcf9aeb64 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -955,7 +955,7 @@ protected: void send(const hrt_abstime t) { mavlink_timesync_t msg; - msg.tc1 = -1; + msg.tc1 = 0; msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 13e17ab64..cd2ef1ffd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -122,7 +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.8f), + _time_offset_avg_alpha(0.75), _time_offset(0) { @@ -1007,22 +1007,24 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) uint64_t now_ns = hrt_absolute_time() * 1000 ; - if (tsync.tc1 == -1) { + if (tsync.tc1 == 0) { - mavlink_timesync_t rsync; // return sync message + mavlink_timesync_t rsync; // return timestamped sync message rsync.tc1 = now_ns; rsync.ts1 = tsync.ts1; _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync); - } else if (tsync.tc1 > -1) { + return; + + } else if (tsync.tc1 > 0) { int64_t offset_ns = ((tsync.ts1 + now_ns) - (tsync.tc1 * 2)) / 2; int64_t dt = _time_offset - offset_ns; - if (dt > 1000000 || dt < -100000) { // 1 millisecond skew XXX Make this - _time_offset = offset_ns; // hard-set it. + if (dt > 10000000 || dt < -1000000) { // 10 millisecond skew + _time_offset = offset_ns; warnx("[timesync] Timesync offset is off. Hard-setting offset"); } else { @@ -1506,7 +1508,7 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec) void MavlinkReceiver::average_time_offset(uint64_t offset_ns) { - /* alpha = 0.8 fixed for now. The closer alpha is to 1.0, + /* 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. */ |