UAVCAN: Refactored and generalized sensor bridge support

9 files changed, 265 insertions, 48 deletions

diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/actuators/esc.cpp
index 406eba88c..223d94731 100644
--- a/src/modules/uavcan/esc_controller.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -32,12 +32,12 @@
  ****************************************************************************/
 
 /**
- * @file esc_controller.cpp
+ * @file esc.cpp
  *
  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
  */
 
-#include "esc_controller.hpp"
+#include "esc.hpp"
 #include <systemlib/err.h>
 
 UavcanEscController::UavcanEscController(uavcan::INode &node) :
diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/actuators/esc.hpp
index 559ede561..cf0988210 100644
--- a/src/modules/uavcan/esc_controller.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file esc_controller.hpp
+ * @file esc.hpp
  *
  * UAVCAN <--> ORB bridge for ESC messages:
  *     uavcan.equipment.esc.RawCommand
diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
index 3865f2468..2b9729045 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -40,10 +40,16 @@ MODULE_COMMAND = uavcan
 
 MAXOPTIMIZATION = -Os
 
-SRCS += uavcan_main.cpp         \
-        uavcan_clock.cpp        \
-        esc_controller.cpp      \
-        gnss_receiver.cpp
+# Main
+SRCS += uavcan_main.cpp              \
+        uavcan_clock.cpp
+
+# Actuators
+SRCS += actuators/esc.cpp
+
+# Sensors
+SRCS += sensors/sensor_bridge.cpp    \
+        sensors/gnss.cpp
 
 #
 # libuavcan
diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/sensors/gnss.cpp
index ba1fe5e49..4fc0743ff 100644
--- a/src/modules/uavcan/gnss_receiver.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -32,32 +32,34 @@
  ****************************************************************************/
 
 /**
- * @file gnss_receiver.cpp
+ * @file gnss.cpp
  *
  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
  * @author Andrew Chambers <achamber@gmail.com>
  *
  */
 
-#include "gnss_receiver.hpp"
+#include "gnss.hpp"
 #include <systemlib/err.h>
 #include <mathlib/mathlib.h>
 
 #define MM_PER_CM 			10	// Millimeters per centimeter
 
-UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) :
+UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
 _node(node),
 _uavcan_sub_status(node),
 _report_pub(-1)
 {
 }
 
-int UavcanGnssReceiver::init()
+const char *UavcanGnssBridge::get_name() const { return "gnss"; }
+
+int UavcanGnssBridge::init()
 {
 	int res = -1;
 
 	// GNSS fix subscription
-	res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb));
+	res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
 	if (res < 0)
 	{
 		warnx("GNSS fix sub failed %i", res);
@@ -67,10 +69,11 @@ int UavcanGnssReceiver::init()
 	// Clear the uORB GPS report
 	memset(&_report, 0, sizeof(_report));
 
+	warnx("gnss sensor bridge init ok");
 	return res;
 }
 
-void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
+void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
 {
 	_report.timestamp_position = hrt_absolute_time();
 	_report.lat = msg.lat_1e7;
diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/sensors/gnss.hpp
index 18df8da2f..eec595ad1 100644
--- a/src/modules/uavcan/gnss_receiver.hpp
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file gnss_receiver.hpp
+ * @file gnss.hpp
  *
  * UAVCAN --> ORB bridge for GNSS messages:
  *     uavcan.equipment.gnss.Fix
@@ -51,12 +51,16 @@
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/gnss/Fix.hpp>
 
-class UavcanGnssReceiver
+#include "sensor_bridge.hpp"
+
+class UavcanGnssBridge : public IUavcanSensorBridge
 {
 public:
-	UavcanGnssReceiver(uavcan::INode& node);
+	UavcanGnssBridge(uavcan::INode& node);
+
+	const char *get_name() const override;
 
-	int init();
+	int init() override;
 
 private:
 	/**
@@ -65,14 +69,14 @@ private:
 	void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
 
 
-	typedef uavcan::MethodBinder<UavcanGnssReceiver*,
-		void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
+	typedef uavcan::MethodBinder<UavcanGnssBridge*,
+		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
 		FixCbBinder;
 
 	/*
 	 * libuavcan related things
 	 */
-	uavcan::INode													&_node;
+	uavcan::INode							&_node;
 	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder>	_uavcan_sub_status;
 
 	/*
diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp
new file mode 100644
index 000000000..ba27d6c71
--- /dev/null
+++ b/src/modules/uavcan/sensors/sensor_bridge.cpp
@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include "sensor_bridge.hpp"
+#include "gnss.hpp"
+
+IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name)
+{
+	if (!std::strncmp("gnss", bridge_name, MaxNameLen)) {
+		return new UavcanGnssBridge(node);
+	} else {
+		return nullptr;
+	}
+}
diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp
new file mode 100644
index 000000000..1667c2b57
--- /dev/null
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <containers/List.hpp>
+#include <uavcan/uavcan.hpp>
+
+/**
+ * A sensor bridge class must implement this interface.
+ */
+class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge*>
+{
+public:
+	static constexpr unsigned MaxNameLen = 20;
+
+	virtual ~IUavcanSensorBridge() { }
+
+	/**
+	 * Returns ASCII name of the bridge.
+	 */
+	virtual const char *get_name() const = 0;
+
+	/**
+	 * Starts the bridge.
+	 * @return Non-negative value on success, negative on error.
+	 */
+	virtual int init() = 0;
+
+	/**
+	 * Sensor bridge factory.
+	 * Creates a bridge object by its ASCII name, e.g. "gnss", "mag".
+	 * @return nullptr if such bridge can't be created.
+	 */
+	static IUavcanSensorBridge* make(uavcan::INode &node, const char *bridge_name);
+};
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 4535b6d6a..fc5521aa7 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -65,16 +65,18 @@ UavcanNode *UavcanNode::_instance;
 UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
 	CDev("uavcan", UAVCAN_DEVICE_PATH),
 	_node(can_driver, system_clock),
-	_esc_controller(_node),
-	_gnss_receiver(_node)
+	_node_mutex(),
+	_esc_controller(_node)
 {
 	_control_topics[0] = ORB_ID(actuator_controls_0);
 	_control_topics[1] = ORB_ID(actuator_controls_1);
 	_control_topics[2] = ORB_ID(actuator_controls_2);
 	_control_topics[3] = ORB_ID(actuator_controls_3);
 
-	// memset(_controls, 0, sizeof(_controls));
-	// memset(_poll_fds, 0, sizeof(_poll_fds));
+	const int res = pthread_mutex_init(&_node_mutex, nullptr);
+	if (res < 0) {
+		std::abort();
+	}
 }
 
 UavcanNode::~UavcanNode()
@@ -99,7 +101,7 @@ UavcanNode::~UavcanNode()
 	}
 
 	/* clean up the alternate device node */
-		// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+	// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
 
 	::close(_armed_sub);
 
@@ -231,10 +233,6 @@ int UavcanNode::init(uavcan::NodeID node_id)
 	if (ret < 0)
 		return ret;
 
-	ret = _gnss_receiver.init();
-	if (ret < 0)
-		return ret;
-
 	return _node.start();
 }
 
@@ -248,6 +246,8 @@ void UavcanNode::node_spin_once()
 
 int UavcanNode::run()
 {
+	(void)pthread_mutex_lock(&_node_mutex);
+
 	const unsigned PollTimeoutMs = 50;
 
 	// XXX figure out the output count
@@ -291,8 +291,13 @@ int UavcanNode::run()
 			_groups_subscribed = _groups_required;
 		}
 
+		// Mutex is unlocked while the thread is blocked on IO multiplexing
+		(void)pthread_mutex_unlock(&_node_mutex);
+
 		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
 
+		(void)pthread_mutex_lock(&_node_mutex);
+
 		node_spin_once();  // Non-blocking
 
 		// this would be bad...
@@ -352,7 +357,6 @@ int UavcanNode::run()
 				// Output to the bus
 				_esc_controller.update_outputs(outputs.output, outputs.noutputs);
 			}
-
 		}
 
 		// Check arming state
@@ -376,10 +380,7 @@ int UavcanNode::run()
 }
 
 int
-UavcanNode::control_callback(uintptr_t handle,
-			 uint8_t control_group,
-			 uint8_t control_index,
-			 float &input)
+UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
 {
 	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
@@ -524,12 +525,69 @@ UavcanNode::print_info()
 	warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
 }
 
+int UavcanNode::sensor_enable(const char *bridge_name)
+{
+	int retval = -1;
+
+	(void)pthread_mutex_lock(&_node_mutex);
+
+	// Checking if such bridge already exists
+	{
+		auto pos = _sensor_bridges.getHead();
+		while (pos != nullptr) {
+			if (!std::strncmp(pos->get_name(), bridge_name, IUavcanSensorBridge::MaxNameLen)) {
+				warnx("sensor bridge '%s' already exists", bridge_name);
+				retval = -1;
+				goto leave;
+			}
+			pos = pos->getSibling();
+		}
+	}
+
+	// Creating and initing
+	{
+		auto bridge = IUavcanSensorBridge::make(get_node(), bridge_name);
+		if (bridge == nullptr) {
+			warnx("cannot create sensor bridge '%s'", bridge_name);
+			retval = -1;
+			goto leave;
+		}
+
+		assert(!std::strncmp(bridge->get_name(), bridge_name, IUavcanSensorBridge::MaxNameLen));
+
+		retval = bridge->init();
+		if (retval >= 0) {
+			_sensor_bridges.add(bridge);
+		}
+	}
+
+leave:
+	(void)pthread_mutex_unlock(&_node_mutex);
+	return retval;
+}
+
+void UavcanNode::sensor_print_enabled()
+{
+	(void)pthread_mutex_lock(&_node_mutex);
+
+	auto pos = _sensor_bridges.getHead();
+	while (pos != nullptr) {
+		warnx("%s", pos->get_name());
+		pos = pos->getSibling();
+	}
+
+	(void)pthread_mutex_unlock(&_node_mutex);
+}
+
 /*
  * App entry point
  */
 static void print_usage()
 {
-	warnx("usage: uavcan start <node_id> [can_bitrate]");
+	warnx("usage: \n"
+	      "\tuavcan start <node_id> [can_bitrate]\n"
+	      "\tuavcan sensor enable <sensor name>\n"
+	      "\tuavcan sensor list");
 }
 
 extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -585,7 +643,7 @@ int uavcan_main(int argc, char *argv[])
 	}
 
 	/* commands below require the app to be started */
-	UavcanNode *inst = UavcanNode::instance();
+	UavcanNode *const inst = UavcanNode::instance();
 
 	if (!inst) {
 		errx(1, "application not running");
@@ -594,14 +652,37 @@ int uavcan_main(int argc, char *argv[])
 	if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
 		
 			inst->print_info();
-			return OK;
+			::exit(0);
 	}
 
 	if (!std::strcmp(argv[1], "stop")) {
-		
 			delete inst;
-			inst = nullptr;
-			return OK;
+			::exit(0);
+	}
+
+	if (!std::strcmp(argv[1], "sensor")) {
+		if (argc < 3) {
+			print_usage();
+			::exit(1);
+		}
+
+		if (!std::strcmp(argv[2], "list")) {
+			inst->sensor_print_enabled();
+			::exit(0);
+		}
+
+		if (argc < 4) {
+			print_usage();
+			::exit(1);
+		}
+
+		if (!std::strcmp(argv[2], "enable")) {
+			const int res = inst->sensor_enable(argv[3]);
+			if (res < 0) {
+				warnx("failed to enable sensor '%s': error %d", argv[3], res);
+			}
+			::exit(0);
+		}
 	}
 
 	print_usage();
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 05b66fd7b..bca1aa530 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -42,8 +42,8 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 
-#include "esc_controller.hpp"
-#include "gnss_receiver.hpp"
+#include "actuators/esc.hpp"
+#include "sensors/sensor_bridge.hpp"
 
 /**
  * @file uavcan_main.hpp
@@ -77,12 +77,10 @@ public:
 
 	static int	start(uavcan::NodeID node_id, uint32_t bitrate);
 
-	Node&		getNode() { return _node; }
+	Node&		get_node() { return _node; }
 
-	static int	control_callback(uintptr_t handle,
-					 uint8_t control_group,
-					 uint8_t control_index,
-					 float &input);
+	// TODO: move the actuator mixing stuff into the ESC controller class
+	static int	control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
 
 	void		subscribe();
 
@@ -91,6 +89,10 @@ public:
 
 	void		print_info();
 
+	int		sensor_enable(const char *bridge_name);
+
+	void		sensor_print_enabled();
+
 	static UavcanNode* instance() { return _instance; }
 
 private:
@@ -109,8 +111,11 @@ private:
 
 	static UavcanNode	*_instance;			///< singleton pointer
 	Node			_node;				///< library instance
+	pthread_mutex_t		_node_mutex;
+
 	UavcanEscController	_esc_controller;
-	UavcanGnssReceiver 	_gnss_receiver;
+
+	List<IUavcanSensorBridge*> _sensor_bridges;		///< Append-only list of active sensor bridges
 
 	MixerGroup		*_mixers = nullptr;