uas_timesync.cpp
Go to the documentation of this file.
1 
6 /*
7  * Copyright 2014,2017 Vladimir Ermakov, M.H.Kabir.
8  *
9  * This file is part of the mavros package and subject to the license terms
10  * in the top-level LICENSE file of the mavros repository.
11  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
12  */
13 
14 #include <array>
15 #include <unordered_map>
16 #include <stdexcept>
17 #include <mavros/mavros_uas.h>
18 #include <mavros/utils.h>
19 #include <mavros/px4_custom_mode.h>
20 
21 using namespace mavros;
22 
23 
24 /* -*- time syncronise functions -*- */
25 
26 static inline ros::Time ros_time_from_ns(const uint64_t stamp_ns) {
27  return ros::Time(
28  stamp_ns / 1000000000UL, // t_sec
29  stamp_ns % 1000000000UL); // t_nsec
30 }
31 
32 ros::Time UAS::synchronise_stamp(uint32_t time_boot_ms) {
33  // copy offset from atomic var
34  uint64_t offset_ns = time_offset;
35 
36  if (offset_ns > 0 || tsync_mode == timesync_mode::PASSTHROUGH) {
37  uint64_t stamp_ns = static_cast<uint64_t>(time_boot_ms) * 1000000UL + offset_ns;
38  return ros_time_from_ns(stamp_ns);
39  }
40  else
41  return ros::Time::now();
42 }
43 
44 ros::Time UAS::synchronise_stamp(uint64_t time_usec) {
45  uint64_t offset_ns = time_offset;
46 
47  if (offset_ns > 0 || tsync_mode == timesync_mode::PASSTHROUGH) {
48  uint64_t stamp_ns = time_usec * 1000UL + offset_ns;
49  return ros_time_from_ns(stamp_ns);
50  }
51  else
52  return ros::Time::now();
53 }
54 
static ros::Time ros_time_from_ns(const uint64_t stamp_ns)
ros::Time synchronise_stamp(uint32_t time_boot_ms)
Compute FCU message time from time_boot_ms or time_usec field.
MAVROS Plugin context.
timesync_mode tsync_mode
Definition: mavros_uas.h:420
PX4 custom mode constantsPX4 custom flight modes.
static Time now()
std::atomic< uint64_t > time_offset
Definition: mavros_uas.h:419


mavros
Author(s): Vladimir Ermakov
autogenerated on Mon Jul 8 2019 03:20:11