From d69be4b554dbaead3fba5d42c2e5de1e056acf37 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Sat, 8 Aug 2015 08:00:00 -1000
Subject: [PATCH] Added UAVCAN Time Synchronization Master capabilities to FMU

---
 src/modules/uavcan/module.mk       |  2 +-
 src/modules/uavcan/uavcan_main.cpp | 68 +++++++++++++++++++++++++++++-
 src/modules/uavcan/uavcan_main.hpp | 14 +++++-
 3 files changed, 81 insertions(+), 3 deletions(-)

diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
index f846fd27bc..77ff77c6d7 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -70,7 +70,7 @@ override EXTRADEFINES := $(EXTRADEFINES) \
 -DUAVCAN_NO_ASSERTIONS \
 -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \
 -DUAVCAN_MAX_NETWORK_SIZE_HINT=16 \
--DUAVCAN_STM32_TIMER_NUMBER=2
+-DUAVCAN_STM32_TIMER_NUMBER=5
 
 #
 # libuavcan drivers for STM32
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 5e4c8b228f..9daff4b075 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -77,7 +77,10 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	CDev("uavcan", UAVCAN_DEVICE_PATH),
 	_node(can_driver, system_clock),
 	_node_mutex(),
-	_esc_controller(_node)
+	_esc_controller(_node),
+	_time_sync_master(_node),
+	_time_sync_slave(_node),
+	_master_timer(_node)
 {
 	_task_should_exit = false;
 	_fw_server_action = None;
@@ -459,6 +462,48 @@ int UavcanNode::add_poll_fd(int fd)
 }
 
 
+void UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
+{
+
+	/*
+	 * Check whether there are higher priority masters in the network.
+	 * If there are, we need to activate the local slave in order to sync with them.
+	 */
+	if (_time_sync_slave.isActive()) { // "Active" means that the slave tracks at least one remote master in the network
+		if (_node.getNodeID() < _time_sync_slave.getMasterNodeID()) {
+			/*
+			 * We're the highest priority master in the network.
+			 * We need to suppress the slave now to prevent it from picking up unwanted sync messages from
+			 * lower priority masters.
+			 */
+			_time_sync_slave.suppress(true);  // SUPPRESS
+
+		} else {
+			/*
+			 * There is at least one higher priority master in the network.
+			 * We need to allow the slave to adjust our local clock in order to be in sync.
+			 */
+			_time_sync_slave.suppress(false);  // UNSUPPRESS
+		}
+
+	} else {
+		/*
+		 * There are no other time sync masters in the network, so we're the only time source.
+		 * The slave must be suppressed anyway to prevent it from disrupting the local clock if a new
+		 * lower priority master suddenly appears in the network.
+		 */
+		_time_sync_slave.suppress(true);
+	}
+
+	/*
+	 * Publish the sync message now, even if we're not a higher priority master.
+	 * Other nodes will be able to pick the right master anyway.
+	 */
+	_time_sync_master.publish();
+}
+
+
+
 int UavcanNode::run()
 {
 	(void)pthread_mutex_lock(&_node_mutex);
@@ -472,6 +517,27 @@ int UavcanNode::run()
 
 	memset(&_outputs, 0, sizeof(_outputs));
 
+        /*
+         * Set up the time synchronization
+         */
+
+        const int slave_init_res = _time_sync_slave.start();
+        if (slave_init_res < 0)
+        {
+            warnx("Failed to start time_sync_slave");
+            _task_should_exit = true;
+        }
+
+        /* When we have a system wide notion of time update (i.e the transition from the initial
+         * System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that
+         * happens, but for now we use adjustUtc with a correction of 0
+         */
+        uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0));
+        _master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync));
+        _master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000));
+
+
+
 	const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
 
 	if (busevent_fd < 0) {
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index acc1c32843..098d8ea55d 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -36,6 +36,9 @@
 #include <px4_config.h>
 
 #include <uavcan_stm32/uavcan_stm32.hpp>
+#include <uavcan/protocol/global_time_sync_master.hpp>
+#include <uavcan/protocol/global_time_sync_slave.hpp>
+
 #include <drivers/device/device.h>
 #include <systemlib/perf_counter.h>
 
@@ -48,7 +51,7 @@
 #include "actuators/esc.hpp"
 #include "sensors/sensor_bridge.hpp"
 
-# include "uavcan_servers.hpp"
+#include "uavcan_servers.hpp"
 
 /**
  * @file uavcan_main.hpp
@@ -121,6 +124,7 @@ public:
 	void            attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
 
 private:
+
 	void		fill_node_info();
 	int		init(uavcan::NodeID node_id);
 	void		node_spin_once();
@@ -150,6 +154,8 @@ private:
 	pthread_mutex_t		_node_mutex;
 	sem_t                   _server_command_sem;
 	UavcanEscController	_esc_controller;
+	uavcan::GlobalTimeSyncMaster _time_sync_master;
+	uavcan::GlobalTimeSyncSlave _time_sync_slave;
 
 	List<IUavcanSensorBridge *> _sensor_bridges;		///< List of active sensor bridges
 
@@ -175,4 +181,10 @@ private:
 	perf_counter_t _perfcnt_node_spin_elapsed        = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed");
 	perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed");
 	perf_counter_t _perfcnt_esc_mixer_total_elapsed  = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed");
+
+	void handle_time_sync(const uavcan::TimerEvent &);
+
+	typedef uavcan::MethodBinder<UavcanNode *, void (UavcanNode::*)(const uavcan::TimerEvent &)> TimerCallback;
+	uavcan::TimerEventForwarder<TimerCallback> _master_timer;
+
 };