Merge pull request #2895 from PX4/master_uavcan_timesync_with_params

Master uavcan timesync with command line UAVCAN parameter operations

NuttX

@@ -1 +1 @@
-Subproject commit 8891d035df45c8be570cfbd9419b438679faf7ee
+Subproject commit aa15c32058acbda4cc2ee51e97d31e9e49225193

src/lib/uavcan

@@ -1 +1 @@
-Subproject commit 3ae5400aa5ead18139106d30f730114d5e9b65dd
+Subproject commit 50dc08663af2d9b55eae0ccf5f07c63db0ee8907

src/modules/uavcan/module.mk

@@ -40,12 +40,11 @@ MODULE_COMMAND = uavcan
 
 MAXOPTIMIZATION = -O3
 MODULE_STACKSIZE = 3200
-WFRAME_LARGER_THAN = 1400
+WFRAME_LARGER_THAN = 1416
 
 # Main
 SRCS += uavcan_main.cpp              \
-	uavcan_servers.cpp           \
-        uavcan_clock.cpp             \
+				uavcan_servers.cpp           \
         uavcan_params.c
 
 # Actuators
@@ -70,7 +69,8 @@ override EXTRADEFINES := $(EXTRADEFINES) \
 -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \
 -DUAVCAN_NO_ASSERTIONS \
 -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \
--DUAVCAN_MAX_NETWORK_SIZE_HINT=16
+-DUAVCAN_MAX_NETWORK_SIZE_HINT=16 \
+-DUAVCAN_STM32_TIMER_NUMBER=5
 
 #
 # libuavcan drivers for STM32

src/modules/uavcan/uavcan_clock.cpp

@@ -1,81 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include <uavcan_stm32/uavcan_stm32.hpp>
-#include <drivers/drv_hrt.h>
-
-/**
- * @file uavcan_clock.cpp
- *
- * Implements a clock for the CAN node.
- *
- * @author Pavel Kirienko <pavel.kirienko@gmail.com>
- */
-
-namespace uavcan_stm32
-{
-namespace clock
-{
-
-uavcan::MonotonicTime getMonotonic()
-{
-	return uavcan::MonotonicTime::fromUSec(hrt_absolute_time());
-}
-
-uavcan::UtcTime getUtc()
-{
-	return uavcan::UtcTime();
-}
-
-void adjustUtc(uavcan::UtcDuration adjustment)
-{
-	(void)adjustment;
-}
-
-uavcan::uint64_t getUtcUSecFromCanInterrupt();
-
-uavcan::uint64_t getUtcUSecFromCanInterrupt()
-{
-	return 0;
-}
-
-} // namespace clock
-
-SystemClock &SystemClock::instance()
-{
-	static SystemClock inst;
-	return inst;
-}
-
-}
-

src/modules/uavcan/uavcan_main.cpp

@@ -55,6 +55,8 @@
 #include "uavcan_main.hpp"
 #include <uavcan/util/templates.hpp>
 
+#include <uavcan/protocol/param/ExecuteOpcode.hpp>
+
 //todo:The Inclusion of file_server_backend is killing
 // #include <sys/types.h> and leaving OK undefined
 # define OK 0
@@ -77,7 +79,11 @@ 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),
+	_setget_response(0)
 {
 	_task_should_exit = false;
 	_fw_server_action = None;
@@ -186,6 +192,239 @@ int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
 	return rv;
 }
 
+int UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp)
+{
+	if (resp.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
+		return std::printf("name: %s %lld\n", resp.name.c_str(),
+				   resp.value.to<uavcan::protocol::param::Value::Tag::integer_value>());
+
+	} else if (resp.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
+		return   std::printf("name: %s %.4f\n", resp.name.c_str(),
+				     static_cast<double>(resp.value.to<uavcan::protocol::param::Value::Tag::real_value>()));
+
+	} else if (resp.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
+		return std::printf("name: %s %d\n", resp.name.c_str(),
+				   resp.value.to<uavcan::protocol::param::Value::Tag::boolean_value>());
+
+	} else if (resp.value.is(uavcan::protocol::param::Value::Tag::string_value)) {
+		return std::printf("name: %s '%s'\n", resp.name.c_str(),
+				   resp.value.to<uavcan::protocol::param::Value::Tag::string_value>().c_str());
+	}
+
+	return -1;
+}
+
+void UavcanNode::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
+{
+	uavcan::protocol::param::ExecuteOpcode::Response resp;
+	_callback_success = result.isSuccessful();
+	resp = result.getResponse();
+	_callback_success &= resp.ok;
+}
+
+int  UavcanNode::save_params(int remote_node_id)
+{
+	uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
+	opcode_req.opcode = opcode_req.OPCODE_SAVE;
+	uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> client(_node);
+	client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode));
+	_callback_success = false;
+	int call_res = client.call(remote_node_id, opcode_req);
+
+	if (call_res >= 0) {
+		while (client.hasPendingCalls()) {
+			usleep(10000);
+		}
+	}
+
+	if (!_callback_success) {
+		std::printf("Failed to save parameters: %d\n", call_res);
+		return -1;
+	}
+
+	return 0;
+}
+
+void UavcanNode::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
+{
+	uavcan::protocol::RestartNode::Response resp;
+	_callback_success = result.isSuccessful();
+	resp = result.getResponse();
+	_callback_success &= resp.ok;
+}
+
+int  UavcanNode::reset_node(int remote_node_id)
+{
+	uavcan::protocol::RestartNode::Request restart_req;
+	restart_req.magic_number = restart_req.MAGIC_NUMBER;
+	uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> client(_node);
+	client.setCallback(RestartNodeCallback(this, &UavcanNode::cb_restart));
+	_callback_success = false;
+	int call_res = client.call(remote_node_id, restart_req);
+
+	if (call_res >= 0) {
+		while (client.hasPendingCalls()) {
+			usleep(10000);
+		}
+	}
+
+	if (!call_res) {
+		std::printf("Failed to reset node: %d\n", remote_node_id);
+		return -1;
+	}
+
+	return 0;
+}
+
+int  UavcanNode::list_params(int remote_node_id)
+{
+	int rv = 0;
+	int index = 0;
+	uavcan::protocol::param::GetSet::Response resp;
+	set_setget_response(&resp);
+
+	while (true) {
+		uavcan::protocol::param::GetSet::Request req;
+		req.index =  index++;
+		_callback_success = false;
+		int call_res = get_set_param(remote_node_id, nullptr, req);
+
+		if (call_res < 0 || !_callback_success) {
+			std::printf("Failed to get param: %d\n", call_res);
+			rv = -1;
+			break;
+		}
+
+		if (resp.name.empty()) { // Empty name means no such param, which means we're finished
+			break;
+		}
+
+		print_params(resp);
+	}
+
+	free_setget_response();
+	return rv;
+}
+
+
+void UavcanNode::cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
+{
+	_callback_success = result.isSuccessful();
+	*_setget_response = result.getResponse();
+}
+
+int  UavcanNode::get_set_param(int remote_node_id, const char *name, uavcan::protocol::param::GetSet::Request &req)
+{
+	if (name != nullptr) {
+		req.name = name;
+	}
+
+	uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> client(_node);
+	client.setCallback(GetSetCallback(this, &UavcanNode::cb_setget));
+	_callback_success = false;
+	int call_res = client.call(remote_node_id, req);
+
+	if (call_res >= 0) {
+
+		while (client.hasPendingCalls()) {
+			usleep(10000);
+		}
+
+		if (!_callback_success) {
+			call_res = -1;
+		}
+	}
+
+	return call_res;
+}
+
+int  UavcanNode::set_param(int remote_node_id, const char *name, char *value)
+{
+	uavcan::protocol::param::GetSet::Request req;
+	uavcan::protocol::param::GetSet::Response resp;
+	set_setget_response(&resp);
+	int rv = get_set_param(remote_node_id, name, req);
+
+	if (rv < 0 || resp.name.empty()) {
+		std::printf("Failed to retrieve param: %s\n", name);
+		rv = -1;
+
+	} else {
+
+		rv = 0;
+		req = {};
+
+		if (resp.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
+			int64_t i = std::strtoull(value, NULL, 10);
+			int64_t min = resp.min_value.to<uavcan::protocol::param::NumericValue::Tag::integer_value>();
+			int64_t max = resp.max_value.to<uavcan::protocol::param::NumericValue::Tag::integer_value>();
+
+			if (i >= min &&  i <= max) {
+				req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = i;
+
+			} else {
+				std::printf("Invalid value for: %s must be between %lld and %lld\n", name, min, max);
+				rv = -1;
+			}
+
+		} else if (resp.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
+			float f = static_cast<float>(std::atof(value));
+			float min = resp.min_value.to<uavcan::protocol::param::NumericValue::Tag::real_value>();
+			float max = resp.max_value.to<uavcan::protocol::param::NumericValue::Tag::real_value>();
+
+			if (f >= min &&  f <= max) {
+				req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = f;
+
+			} else {
+				std::printf("Invalid value for: %s must be between %.4f and %.4f\n", name, static_cast<double>(min),
+					    static_cast<double>(max));
+				rv = -1;
+			}
+
+		} else if (resp.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
+			int8_t i = (value[0] == '1' || value[0] == 't') ? 1 : 0;
+			req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = i;
+
+		} else if (resp.value.is(uavcan::protocol::param::Value::Tag::string_value)) {
+			req.value.to<uavcan::protocol::param::Value::Tag::string_value>() = value;
+		}
+
+		if (rv == 0) {
+			rv = get_set_param(remote_node_id, name, req);
+
+			if (rv < 0 || resp.name.empty()) {
+				std::printf("Failed to set param: %s\n", name);
+				return -1;
+			}
+
+			return 0;
+		}
+	}
+
+	free_setget_response();
+	return rv;
+}
+
+int  UavcanNode::get_param(int remote_node_id, const char *name)
+{
+	uavcan::protocol::param::GetSet::Request req;
+	uavcan::protocol::param::GetSet::Response resp;
+	set_setget_response(&resp);
+	int rv = get_set_param(remote_node_id, name, req);
+
+	if (rv < 0 || resp.name.empty()) {
+		std::printf("Failed to get param: %s\n", name);
+		rv = -1;
+
+	} else {
+		print_params(resp);
+		rv = 0;
+	}
+
+	free_setget_response();
+	return rv;
+}
+
 int UavcanNode::start_fw_server()
 {
 	int rv = -1;
@@ -459,6 +698,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 +753,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) {
@@ -870,7 +1172,7 @@ UavcanNode::print_info()
 static void print_usage()
 {
 	warnx("usage: \n"
-	      "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw}");
+	      "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}");
 }
 
 extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -956,6 +1258,60 @@ int uavcan_main(int argc, char *argv[])
 		::exit(0);
 	}
 
+	/*
+	 * Parameter setting commands
+	 *
+	 *  uavcan param list <node>
+	 *  uavcan param save <node>
+	 *  uavcan param get <node> <name>
+	 *  uavcan param set <node> <name> <value>
+	 *
+	 */
+	int node_arg = !std::strcmp(argv[1], "reset") ? 2 : 3;
+
+	if (!std::strcmp(argv[1], "param") || node_arg == 2) {
+		if (argc < node_arg + 1) {
+			errx(1, "Node id required");
+		}
+
+		int nodeid = atoi(argv[node_arg]);
+
+		if (nodeid  == 0 || nodeid  > 127 || nodeid  == inst->get_node().getNodeID().get()) {
+			errx(1, "Invalid Node id");
+		}
+
+		if (node_arg == 2) {
+
+			return inst->reset_node(nodeid);
+
+		} else if (!std::strcmp(argv[2], "list")) {
+
+			return inst->list_params(nodeid);
+
+		} else if (!std::strcmp(argv[2], "save")) {
+
+			return inst->save_params(nodeid);
+
+		} else if (!std::strcmp(argv[2], "get")) {
+			if (argc < 5) {
+				errx(1, "Name required");
+			}
+
+			return inst->get_param(nodeid, argv[4]);
+
+		} else if (!std::strcmp(argv[2], "set")) {
+			if (argc < 5) {
+				errx(1, "Name required");
+			}
+
+			if (argc < 6) {
+				errx(1, "Value required");
+			}
+
+			return inst->set_param(nodeid, argv[4], argv[5]);
+		}
+	}
+
 	if (!std::strcmp(argv[1], "stop")) {
 		if (fw) {
 

src/modules/uavcan/uavcan_main.hpp

@@ -36,6 +36,12 @@
 #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 <uavcan/protocol/param/GetSet.hpp>
+#include <uavcan/protocol/param/ExecuteOpcode.hpp>
+#include <uavcan/protocol/RestartNode.hpp>
+
 #include <drivers/device/device.h>
 #include <systemlib/perf_counter.h>
 
@@ -48,7 +54,7 @@
 #include "actuators/esc.hpp"
 #include "sensors/sensor_bridge.hpp"
 
-# include "uavcan_servers.hpp"
+#include "uavcan_servers.hpp"
 
 /**
  * @file uavcan_main.hpp
@@ -119,8 +125,13 @@ public:
 	static int         getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
 	int             fw_server(eServerAction action);
 	void            attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
-
+	int             list_params(int remote_node_id);
+	int             save_params(int remote_node_id);
+	int             set_param(int remote_node_id, const char *name, char *value);
+	int             get_param(int remote_node_id, const char *name);
+	int             reset_node(int remote_node_id);
 private:
+
 	void		fill_node_info();
 	int		init(uavcan::NodeID node_id);
 	void		node_spin_once();
@@ -129,6 +140,16 @@ private:
 	int             start_fw_server();
 	int             stop_fw_server();
 	int             request_fw_check();
+	int             print_params(uavcan::protocol::param::GetSet::Response &resp);
+	int             get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req);
+	void            set_setget_response(uavcan::protocol::param::GetSet::Response *resp)
+	{
+		_setget_response = resp;
+	}
+	void            free_setget_response(void)
+	{
+		_setget_response = nullptr;
+	}
 
 	int			_task = -1;			///< handle to the OS task
 	bool			_task_should_exit = false;	///< flag to indicate to tear down the CAN driver
@@ -150,6 +171,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 +198,22 @@ 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;
+
+	bool _callback_success;
+	uavcan::protocol::param::GetSet::Response *_setget_response;
+	typedef uavcan::MethodBinder<UavcanNode *,
+		void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
+	typedef uavcan::MethodBinder<UavcanNode *,
+		void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)> ExecuteOpcodeCallback;
+	typedef uavcan::MethodBinder<UavcanNode *,
+		void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
+	void cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+	void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
+	void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
+
 };