Browse Source

Working now.

sbg
M.H.Kabir 10 years ago
parent
commit
8c0d7047b2
  1. 2
      src/modules/mavlink/mavlink_messages.cpp
  2. 16
      src/modules/mavlink/mavlink_receiver.cpp

2
src/modules/mavlink/mavlink_messages.cpp

@ -955,7 +955,7 @@ protected:
void send(const hrt_abstime t) { void send(const hrt_abstime t) {
mavlink_timesync_t msg; mavlink_timesync_t msg;
msg.tc1 = -1; msg.tc1 = 0;
msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds
_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg); _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg);

16
src/modules/mavlink/mavlink_receiver.cpp

@ -122,7 +122,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_hil_local_proj_inited(0), _hil_local_proj_inited(0),
_hil_local_alt0(0.0f), _hil_local_alt0(0.0f),
_hil_local_proj_ref {}, _hil_local_proj_ref {},
_time_offset_avg_alpha(0.8f), _time_offset_avg_alpha(0.75),
_time_offset(0) _time_offset(0)
{ {
@ -1007,22 +1007,24 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
uint64_t now_ns = hrt_absolute_time() * 1000 ; 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.tc1 = now_ns;
rsync.ts1 = tsync.ts1; rsync.ts1 = tsync.ts1;
_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync); _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 offset_ns = ((tsync.ts1 + now_ns) - (tsync.tc1 * 2)) / 2;
int64_t dt = _time_offset - offset_ns; int64_t dt = _time_offset - offset_ns;
if (dt > 1000000 || dt < -100000) { // 1 millisecond skew XXX Make this if (dt > 10000000 || dt < -1000000) { // 10 millisecond skew
_time_offset = offset_ns; // hard-set it. _time_offset = offset_ns;
warnx("[timesync] Timesync offset is off. Hard-setting offset"); warnx("[timesync] Timesync offset is off. Hard-setting offset");
} else { } else {
@ -1506,7 +1508,7 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec)
void MavlinkReceiver::average_time_offset(uint64_t offset_ns) 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 * the faster the moving average updates in response to
* new offset samples. * new offset samples.
*/ */

Loading…
Cancel
Save