fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 23:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Working now.

Browse Source
This commit is contained in:
M.H.Kabir
2014-12-16 13:43:48 +05:30
parent e1bdc4a0fb
commit 8c0d7047b2
2 changed files with 10 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+1 -1
View File
@@ -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);
src/modules/mavlink/mavlink_receiver.cpp
+9 -7
View File
@@ -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.
*/