uas_timesync.cpp
Go to the documentation of this file.
00001 
00006 /*
00007  * Copyright 2014 Vladimir Ermakov.
00008  *
00009  * This file is part of the mavros package and subject to the license terms
00010  * in the top-level LICENSE file of the mavros repository.
00011  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00012  */
00013 
00014 #include <array>
00015 #include <unordered_map>
00016 #include <stdexcept>
00017 #include <mavros/mavros_uas.h>
00018 #include <mavros/utils.h>
00019 #include <mavros/px4_custom_mode.h>
00020 
00021 using namespace mavros;
00022 
00023 
00024 /* -*- time syncronise functions -*- */
00025 
00026 static inline ros::Time ros_time_from_ns(uint64_t &stamp_ns) {
00027         return ros::Time(
00028                 stamp_ns / 1000000000UL,                // t_sec
00029                 stamp_ns % 1000000000UL);               // t_nsec
00030 }
00031 
00032 ros::Time UAS::synchronise_stamp(uint32_t time_boot_ms) {
00033         // copy offset from atomic var
00034         uint64_t offset_ns = time_offset;
00035 
00036         if (offset_ns > 0) {
00037                 uint64_t stamp_ns = static_cast<uint64_t>(time_boot_ms) * 1000000UL + offset_ns;
00038                 return ros_time_from_ns(stamp_ns);
00039         }
00040         else
00041                 return ros::Time::now();
00042 }
00043 
00044 ros::Time UAS::synchronise_stamp(uint64_t time_usec) {
00045         uint64_t offset_ns = time_offset;
00046 
00047         if (offset_ns > 0) {
00048                 uint64_t stamp_ns = time_usec * 1000UL + offset_ns;
00049                 return ros_time_from_ns(stamp_ns);
00050         }
00051         else
00052                 return ros::Time::now();
00053 }
00054 


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19