45 template <
typename DerivedLogger>
55 time_interface_(time_interface),
56 timer_provider_(timer_provider)
63 template <
typename DerivedLogger>
73 std::chrono::nanoseconds tc1_chrono(tsync.
tc1);
77 std::chrono::nanoseconds ts1_chrono(tsync.
ts1);
78 std::chrono::nanoseconds offset_ns((ts1_chrono + now - 2 * tc1_chrono) / 2);
82 || (
offset_ns_ - offset_ns) < std::chrono::milliseconds(-10))
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);
98 template <
typename DerivedLogger>
104 std::chrono::nanoseconds ns = fcu_time +
offset_ns_;
105 if (ns < std::chrono::nanoseconds::zero())
107 logger_.
error_throttle(1,
"negative time calculated from FCU: fcu_time=%ld, offset_ns=%ld. Using system time",
108 fcu_time, offset_ns_);
114 template <
typename DerivedLogger>
117 mavlink_message_t msg;
void info(const char *format, const Args &...args)
const TimeInterface & time_interface_
std::shared_ptr< TimerInterface > time_sync_timer_
static void mavlink_msg_timesync_decode(const mavlink_message_t *msg, mavlink_timesync_t *timesync)
Decode a timesync message into a struct.
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)
#define MAVLINK_MSG_ID_TIMESYNC
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_
TimeManager(MavlinkComm *comm, LoggerInterface< DerivedLogger > &logger, const TimeInterface &time_interface, TimerProviderInterface &timer_provider)
virtual std::chrono::nanoseconds now() const =0
TimerProviderInterface & timer_provider_
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_
void send_message(const mavlink_message_t &msg)
Send a mavlink message.
static uint16_t mavlink_msg_timesync_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t *msg, int64_t tc1, int64_t ts1)
Pack a timesync message.