UAVCAN: redundant sensors support

9 files changed, 188 insertions, 112 deletions

diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
index 93a1bf96f..f92bc754f 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -49,9 +49,9 @@ SRCS += uavcan_main.cpp              \
 SRCS += actuators/esc.cpp
 
 # Sensors
-SRCS += sensors/sensor_bridge.cpp    \
-        sensors/gnss.cpp             \
-        sensors/mag.cpp              \
+SRCS += sensors/sensor_bridge.cpp   \
+        sensors/gnss.cpp            \
+        sensors/mag.cpp             \
         sensors/baro.cpp
 
 #
diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
index 3afcc3f1c..ef4f0dbba 100644
--- a/src/modules/uavcan/sensors/baro.cpp
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -38,49 +38,26 @@
 #include "baro.hpp"
 #include <cmath>
 
+static const orb_id_t BARO_TOPICS[2] = {
+	ORB_ID(sensor_baro0),
+	ORB_ID(sensor_baro1)
+};
+
 const char *const UavcanBarometerBridge::NAME = "baro";
 
 UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) :
-device::CDev("uavcan_baro", "/dev/uavcan/baro"),
+UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS),
 _sub_air_data(node)
 {
 }
 
-UavcanBarometerBridge::~UavcanBarometerBridge()
-{
-	if (_class_instance > 0) {
-		(void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance);
-	}
-}
-
 int UavcanBarometerBridge::init()
 {
-	// Init the libuavcan subscription
 	int res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb));
 	if (res < 0) {
 		log("failed to start uavcan sub: %d", res);
 		return res;
 	}
-
-	// Detect our device class
-	_class_instance = register_class_devname(BARO_DEVICE_PATH);
-	switch (_class_instance) {
-	case CLASS_DEVICE_PRIMARY: {
-		_orb_id = ORB_ID(sensor_baro0);
-		break;
-	}
-	case CLASS_DEVICE_SECONDARY: {
-		_orb_id = ORB_ID(sensor_baro1);
-		break;
-	}
-	default: {
-		log("invalid class instance: %d", _class_instance);
-		(void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance);
-		return -1;
-	}
-	}
-
-	log("inited with class instance %d", _class_instance);
 	return 0;
 }
 
@@ -131,17 +108,5 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<
 
 	report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
 
-	/*
-	 * Publish
-	 */
-	if (_orb_advert >= 0) {
-		orb_publish(_orb_id, _orb_advert, &report);
-	} else {
-		_orb_advert = orb_advertise(_orb_id, &report);
-		if (_orb_advert < 0) {
-			log("ADVERT FAIL");
-		} else {
-			log("advertised");
-		}
-	}
+	publish(msg.getSrcNodeID().get(), &report);
 }
diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp
index f6aa01216..9d470219e 100644
--- a/src/modules/uavcan/sensors/baro.hpp
+++ b/src/modules/uavcan/sensors/baro.hpp
@@ -39,17 +39,15 @@
 
 #include "sensor_bridge.hpp"
 #include <drivers/drv_baro.h>
-#include <drivers/device/device.h>
 
 #include <uavcan/equipment/air_data/StaticAirData.hpp>
 
-class UavcanBarometerBridge : public IUavcanSensorBridge, public device::CDev
+class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
 {
 public:
 	static const char *const NAME;
 
 	UavcanBarometerBridge(uavcan::INode& node);
-	~UavcanBarometerBridge() override;
 
 	const char *get_name() const override { return NAME; }
 
@@ -67,7 +65,4 @@ private:
 
 	uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data;
 	unsigned _msl_pressure = 101325;
-	orb_id_t _orb_id = nullptr;
-	orb_advert_t _orb_advert = -1;
-	int _class_instance = -1;
 };
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index 6b69d163f..f2bb28087 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -73,8 +73,23 @@ int UavcanGnssBridge::init()
 	return res;
 }
 
+unsigned UavcanGnssBridge::get_num_redundant_channels() const
+{
+	return (_receiver_node_id < 0) ? 0 : 1;
+}
+
 void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
 {
+	// This bridge does not support redundant GNSS receivers yet.
+	if (_receiver_node_id < 0) {
+		_receiver_node_id = msg.getSrcNodeID().get();
+		warnx("GNSS receiver node ID: %d", _receiver_node_id);
+	} else {
+		if (_receiver_node_id != msg.getSrcNodeID().get()) {
+			return;  // This GNSS receiver is the redundant one, ignore it.
+		}
+	}
+
 	_report.timestamp_position = hrt_absolute_time();
 	_report.lat = msg.lat_1e7;
 	_report.lon = msg.lon_1e7;
diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp
index db3a515fa..9488c5fe5 100644
--- a/src/modules/uavcan/sensors/gnss.hpp
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -64,27 +64,23 @@ public:
 
 	int init() override;
 
+	unsigned get_num_redundant_channels() const override;
+
 private:
 	/**
 	 * GNSS fix message will be reported via this callback.
 	 */
 	void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
 
-
 	typedef uavcan::MethodBinder<UavcanGnssBridge*,
 		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
 		FixCbBinder;
 
-	/*
-	 * libuavcan related things
-	 */
-	uavcan::INode							&_node;
-	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder>	_sub_fix;
+	uavcan::INode &_node;
+	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
+	int _receiver_node_id = -1;
 
-	/*
-	 * uORB
-	 */
-	struct vehicle_gps_position_s 	_report;					///< uORB topic for gnss position
-	orb_advert_t			_report_pub;					///< uORB pub for gnss position
+	struct vehicle_gps_position_s _report;   ///< uORB topic for gnss position
+	orb_advert_t _report_pub;                ///< uORB pub for gnss position
 
 };
diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 4f8a5e104..aaa3a4463 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -37,10 +37,16 @@
 
 #include "mag.hpp"
 
+static const orb_id_t MAG_TOPICS[3] = {
+	ORB_ID(sensor_mag0),
+	ORB_ID(sensor_mag1),
+	ORB_ID(sensor_mag2)
+};
+
 const char *const UavcanMagnetometerBridge::NAME = "mag";
 
 UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
-device::CDev("uavcan_mag", "/dev/uavcan/mag"),
+UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
 _sub_mag(node)
 {
 	_scale.x_scale = 1.0F;
@@ -48,45 +54,13 @@ _sub_mag(node)
 	_scale.z_scale = 1.0F;
 }
 
-UavcanMagnetometerBridge::~UavcanMagnetometerBridge()
-{
-	if (_class_instance > 0) {
-		(void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance);
-	}
-}
-
 int UavcanMagnetometerBridge::init()
 {
-	// Init the libuavcan subscription
 	int res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb));
 	if (res < 0) {
 		log("failed to start uavcan sub: %d", res);
 		return res;
 	}
-
-	// Detect our device class
-	_class_instance = register_class_devname(MAG_DEVICE_PATH);
-	switch (_class_instance) {
-	case CLASS_DEVICE_PRIMARY: {
-		_orb_id = ORB_ID(sensor_mag0);
-		break;
-	}
-	case CLASS_DEVICE_SECONDARY: {
-		_orb_id = ORB_ID(sensor_mag1);
-		break;
-	}
-	case CLASS_DEVICE_TERTIARY: {
-		_orb_id = ORB_ID(sensor_mag2);
-		break;
-	}
-	default: {
-		log("invalid class instance: %d", _class_instance);
-		(void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance);
-		return -1;
-	}
-	}
-
-	log("inited with class instance %d", _class_instance);
 	return 0;
 }
 
@@ -140,14 +114,5 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<ua
 	report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
 	report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
 
-	if (_orb_advert >= 0) {
-		orb_publish(_orb_id, _orb_advert, &report);
-	} else {
-		_orb_advert = orb_advertise(_orb_id, &report);
-		if (_orb_advert < 0) {
-			log("ADVERT FAIL");
-		} else {
-			log("advertised");
-		}
-	}
+	publish(msg.getSrcNodeID().get(), &report);
 }
diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp
index 4bc5129a2..6d413a8f7 100644
--- a/src/modules/uavcan/sensors/mag.hpp
+++ b/src/modules/uavcan/sensors/mag.hpp
@@ -38,18 +38,16 @@
 #pragma once
 
 #include "sensor_bridge.hpp"
-#include <drivers/device/device.h>
 #include <drivers/drv_mag.h>
 
 #include <uavcan/equipment/ahrs/Magnetometer.hpp>
 
-class UavcanMagnetometerBridge : public IUavcanSensorBridge, public device::CDev
+class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
 {
 public:
 	static const char *const NAME;
 
 	UavcanMagnetometerBridge(uavcan::INode& node);
-	~UavcanMagnetometerBridge() override;
 
 	const char *get_name() const override { return NAME; }
 
@@ -67,7 +65,4 @@ private:
 
 	uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
 	mag_scale _scale = {};
-	orb_id_t _orb_id = nullptr;
-	orb_advert_t _orb_advert = -1;
-	int _class_instance = -1;
 };
diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp
index 5752d5524..05d589b58 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.cpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.cpp
@@ -36,12 +36,15 @@
  */
 
 #include "sensor_bridge.hpp"
-#include <systemlib/err.h>
+#include <cassert>
 
 #include "gnss.hpp"
 #include "mag.hpp"
 #include "baro.hpp"
 
+/*
+ * IUavcanSensorBridge
+ */
 IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name)
 {
 	/*
@@ -64,3 +67,91 @@ void IUavcanSensorBridge::print_known_names(const char *prefix)
 	printf("%s%s\n", prefix, UavcanMagnetometerBridge::NAME);
 	printf("%s%s\n", prefix, UavcanBarometerBridge::NAME);
 }
+
+/*
+ * UavcanCDevSensorBridgeBase
+ */
+UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
+{
+	for (unsigned i = 0; i < _max_channels; i++) {
+		if (_channels[i].redunancy_channel_id >= 0) {
+			(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
+		}
+	}
+	delete [] _orb_topics;
+	delete [] _channels;
+}
+
+void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const void *report)
+{
+	Channel *channel = nullptr;
+
+	// Checking if such channel already exists
+	for (unsigned i = 0; i < _max_channels; i++) {
+		if (_channels[i].redunancy_channel_id == redundancy_channel_id) {
+			channel = _channels + i;
+			break;
+		}
+	}
+
+	// No such channel - try to create one
+	if (channel == nullptr) {
+		if (_out_of_channels) {
+			return;           // Give up immediately - saves some CPU time
+		}
+
+		log("adding channel %d...", redundancy_channel_id);
+
+		// Search for the first free channel
+		for (unsigned i = 0; i < _max_channels; i++) {
+			if (_channels[i].redunancy_channel_id < 0) {
+				channel = _channels + i;
+				break;
+			}
+		}
+
+		// No free channels left
+		if (channel == nullptr) {
+			_out_of_channels = true;
+			log("out of channels");
+			return;
+		}
+
+		// Ask the CDev helper which class instance we can take
+		const int class_instance = register_class_devname(_class_devname);
+		if (class_instance < 0 || class_instance >= int(_max_channels)) {
+			_out_of_channels = true;
+			log("out of class instances");
+			(void)unregister_class_devname(_class_devname, class_instance);
+			return;
+		}
+
+		// Publish to the appropriate topic, abort on failure
+		channel->orb_id               = _orb_topics[class_instance];
+		channel->redunancy_channel_id = redundancy_channel_id;
+		channel->class_instance       = class_instance;
+
+		channel->orb_advert = orb_advertise(channel->orb_id, report);
+		if (channel->orb_advert < 0) {
+			log("ADVERTISE FAILED");
+			*channel = Channel();
+			return;
+		}
+
+		log("channel %d class instance %d ok", channel->redunancy_channel_id, channel->class_instance);
+	}
+	assert(channel != nullptr);
+
+	(void)orb_publish(channel->orb_id, channel->orb_advert, report);
+}
+
+unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
+{
+	unsigned out = 0;
+	for (unsigned i = 0; i < _max_channels; i++) {
+		if (_channels[i].redunancy_channel_id >= 0) {
+			out += 1;
+		}
+	}
+	return out;
+}
diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp
index 976d9a03d..d27ccb8a0 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.hpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -39,6 +39,8 @@
 
 #include <containers/List.hpp>
 #include <uavcan/uavcan.hpp>
+#include <drivers/device/device.h>
+#include <drivers/drv_orb_dev.h>
 
 /**
  * A sensor bridge class must implement this interface.
@@ -62,6 +64,11 @@ public:
 	virtual int init() = 0;
 
 	/**
+	 * Returns number of active redundancy channels.
+	 */
+	virtual unsigned get_num_redundant_channels() const = 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.
@@ -73,3 +80,50 @@ public:
 	 */
 	static void print_known_names(const char *prefix);
 };
+
+/**
+ * This is the base class for redundant sensors with an independent ORB topic per each redundancy channel.
+ * For example, sensor_mag0, sensor_mag1, etc.
+ */
+class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev
+{
+	struct Channel
+	{
+		int redunancy_channel_id = -1;
+		orb_id_t orb_id          = nullptr;
+		orb_advert_t orb_advert  = -1;
+		int class_instance       = -1;
+	};
+
+	const unsigned _max_channels;
+	const char *const _class_devname;
+	orb_id_t *const _orb_topics;
+	Channel *const _channels;
+	bool _out_of_channels = false;
+
+protected:
+	template <unsigned MaxChannels>
+	UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
+	                           const orb_id_t (&orb_topics)[MaxChannels]) :
+	device::CDev(name, devname),
+	_max_channels(MaxChannels),
+	_class_devname(class_devname),
+	_orb_topics(new orb_id_t[MaxChannels]),
+	_channels(new Channel[MaxChannels])
+	{
+		memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
+	}
+
+	/**
+	 * Sends one measurement into appropriate ORB topic.
+	 * New redundancy channels will be registered automatically.
+	 * @param redundancy_channel_id Redundant sensor identifier (e.g. UAVCAN Node ID)
+	 * @param report                ORB message object
+	 */
+	void publish(const int redundancy_channel_id, const void *report);
+
+public:
+	virtual ~UavcanCDevSensorBridgeBase();
+
+	unsigned get_num_redundant_channels() const override;
+};