time_manager.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017 Daniel Koch and James Jackson, BYU MAGICC Lab.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright notice, this
9  * list of conditions and the following disclaimer.
10  *
11  * * Redistributions in binary form must reproduce the above copyright notice,
12  * this list of conditions and the following disclaimer in the documentation
13  * and/or other materials provided with the distribution.
14  *
15  * * Neither the name of the copyright holder nor the names of its
16  * contributors may be used to endorse or promote products derived from
17  * this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
36 #include <functional>
37 
42 
43 namespace mavrosflight
44 {
45 template <typename DerivedLogger>
48  const TimeInterface &time_interface,
49  TimerProviderInterface &timer_provider) :
50  comm_(comm),
51  offset_alpha_(0.95),
52  offset_ns_(0),
53  initialized_(false),
54  logger_(logger),
55  time_interface_(time_interface),
56  timer_provider_(timer_provider)
57 {
59  std::function<void()> bound_callback = std::bind(&TimeManager<DerivedLogger>::timer_callback, this);
60  time_sync_timer_ = timer_provider_.create_timer(std::chrono::milliseconds(100), bound_callback);
61 }
62 
63 template <typename DerivedLogger>
64 void TimeManager<DerivedLogger>::handle_mavlink_message(const mavlink_message_t &msg)
65 {
66  std::chrono::nanoseconds now = time_interface_.now();
67 
68  if (msg.msgid == MAVLINK_MSG_ID_TIMESYNC)
69  {
70  mavlink_timesync_t tsync;
71  mavlink_msg_timesync_decode(&msg, &tsync);
72 
73  std::chrono::nanoseconds tc1_chrono(tsync.tc1);
74 
75  if (tsync.tc1 > 0) // check that this is a response, not a request
76  {
77  std::chrono::nanoseconds ts1_chrono(tsync.ts1);
78  std::chrono::nanoseconds offset_ns((ts1_chrono + now - 2 * tc1_chrono) / 2);
79 
80  // if difference > 10ms, use it directly
81  if (!initialized_ || (offset_ns_ - offset_ns) > std::chrono::milliseconds(10)
82  || (offset_ns_ - offset_ns) < std::chrono::milliseconds(-10))
83  {
84  offset_ns_ = offset_ns;
85  logger_.info("Detected time offset of %0.3f s.", std::chrono::duration<double>(offset_ns).count());
86  logger_.debug("FCU time: %0.3f, System time: %0.3f", tsync.tc1 * 1e-9, tsync.ts1 * 1e-9);
87  initialized_ = true;
88  }
89  else // otherwise low-pass filter the offset
90  {
91  offset_ns_ = std::chrono::duration_cast<std::chrono::nanoseconds>(offset_alpha_ * offset_ns
92  + (1.0 - offset_alpha_) * offset_ns_);
93  }
94  }
95  }
96 }
97 
98 template <typename DerivedLogger>
99 std::chrono::nanoseconds TimeManager<DerivedLogger>::fcu_time_to_system_time(std::chrono::nanoseconds fcu_time)
100 {
101  if (!initialized_)
102  return time_interface_.now();
103 
104  std::chrono::nanoseconds ns = fcu_time + offset_ns_;
105  if (ns < std::chrono::nanoseconds::zero())
106  {
107  logger_.error_throttle(1, "negative time calculated from FCU: fcu_time=%ld, offset_ns=%ld. Using system time",
108  fcu_time, offset_ns_);
109  return time_interface_.now();
110  }
111  return ns;
112 }
113 
114 template <typename DerivedLogger>
116 {
117  mavlink_message_t msg;
118  mavlink_msg_timesync_pack(1, 50, &msg, 0, time_interface_.now().count());
119  comm_->send_message(msg);
120 }
121 
122 template class TimeManager<DerivedLoggerType>;
123 
124 } // namespace mavrosflight
void info(const char *format, const Args &...args)
const TimeInterface & time_interface_
Definition: time_manager.h:77
std::shared_ptr< TimerInterface > time_sync_timer_
Definition: time_manager.h:68
virtual void handle_mavlink_message(const mavlink_message_t &msg)
The handler function for mavlink messages to be implemented by derived classes.
void error_throttle(float period, const char *format, const Args &...args)
void register_mavlink_listener(MavlinkListenerInterface *const listener)
Register a listener for mavlink messages.
virtual std::shared_ptr< TimerInterface > create_timer(std::chrono::nanoseconds period, std::function< void()> callback, const bool oneshot=false, const bool autostart=true)=0
std::chrono::nanoseconds offset_ns_
Definition: time_manager.h:72
TimeManager(MavlinkComm *comm, LoggerInterface< DerivedLogger > &logger, const TimeInterface &time_interface, TimerProviderInterface &timer_provider)
virtual std::chrono::nanoseconds now() const =0
TimerProviderInterface & timer_provider_
Definition: time_manager.h:78
void debug(const char *format, const Args &...args)
std::chrono::nanoseconds fcu_time_to_system_time(std::chrono::nanoseconds fcu_time)
Provide an interface for creating timers.
Interface for acquiring system time.
LoggerInterface< DerivedLogger > & logger_
Definition: time_manager.h:76
void send_message(const mavlink_message_t &msg)
Send a mavlink message.


rosflight
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Apr 15 2021 05:09:27