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

timesync: Add uORB topic, general fixes

Browse Source
This commit is contained in:
M.H.Kabir
2015-04-14 13:14:15 +05:30
committed by Mohammed Kabir
parent 3c36a61569
commit 66e6938c6d
4 changed files with 96 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+20 -4
View File
@@ -121,6 +121,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_rc_pub(-1),
_manual_pub(-1),
_land_detector_pub(-1),
_time_offset_pub(-1),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0),
_old_timestamp(0),
@@ -729,8 +730,8 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
// Use the component ID to identify the vision sensor
vision_position.id = msg->compid;
vision_position.timestamp_boot = to_hrt(pos.usec); // Synced time
vision_position.timestamp_computer = pos.usec;
vision_position.timestamp_boot = hrt_absolute_time(); // Monotonic time
vision_position.timestamp_computer = sync_stamp(pos.usec); // Synced time
vision_position.x = pos.x;
vision_position.y = pos.y;
vision_position.z = pos.z;
@@ -1013,6 +1014,9 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
mavlink_timesync_t tsync;
mavlink_msg_timesync_decode(msg, &tsync);
struct time_offset_s tsync_offset;
memset(&tsync_offset, 0, sizeof(tsync_offset));
uint64_t now_ns = hrt_absolute_time() * 1000LL ;
if (tsync.tc1 == 0) {
@@ -1039,6 +1043,15 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
}
}
tsync_offset.offset_ns = _time_offset ;
if (_time_offset_pub < 0) {
_time_offset_pub = orb_advertise(ORB_ID(time_offset), &tsync_offset);
} else {
orb_publish(ORB_ID(time_offset), _time_offset_pub, &tsync_offset);
}
}
void
@@ -1522,9 +1535,12 @@ void MavlinkReceiver::print_status()
}
uint64_t MavlinkReceiver::to_hrt(uint64_t usec)
uint64_t MavlinkReceiver::sync_stamp(uint64_t usec)
{
return usec - (_time_offset / 1000) ;
if(_time_offset > 0)
return usec - (_time_offset / 1000) ;
else
return hrt_absolute_time();
}