Improved UAVCAN status reporting

5 files changed, 55 insertions, 18 deletions

diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index 8548660fe..0d67aad47 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -70,6 +70,16 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const
 	return (_receiver_node_id < 0) ? 0 : 1;
 }
 
+void UavcanGnssBridge::print_status() const
+{
+	printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount());
+	if (_receiver_node_id < 0) {
+		printf("N/A\n");
+	} else {
+		printf("%d\n", _receiver_node_id);
+	}
+}
+
 void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
 {
 	// This bridge does not support redundant GNSS receivers yet.
diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp
index c2b6e4195..e8466b401 100644
--- a/src/modules/uavcan/sensors/gnss.hpp
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -66,6 +66,8 @@ public:
 
 	unsigned get_num_redundant_channels() const override;
 
+	void print_status() const override;
+
 private:
 	/**
 	 * GNSS fix message will be reported via this callback.
diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp
index a98596f9c..9608ce680 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.cpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.cpp
@@ -58,7 +58,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
 {
 	for (unsigned i = 0; i < _max_channels; i++) {
-		if (_channels[i].redunancy_channel_id >= 0) {
+		if (_channels[i].node_id >= 0) {
 			(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
 		}
 	}
@@ -66,13 +66,15 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
 	delete [] _channels;
 }
 
-void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const void *report)
+void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 {
+	assert(report != nullptr);
+
 	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) {
+		if (_channels[i].node_id == node_id) {
 			channel = _channels + i;
 			break;
 		}
@@ -84,11 +86,11 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const
 			return;           // Give up immediately - saves some CPU time
 		}
 
-		log("adding channel %d...", redundancy_channel_id);
+		log("adding channel %d...", node_id);
 
 		// Search for the first free channel
 		for (unsigned i = 0; i < _max_channels; i++) {
-			if (_channels[i].redunancy_channel_id < 0) {
+			if (_channels[i].node_id < 0) {
 				channel = _channels + i;
 				break;
 			}
@@ -111,9 +113,9 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const
 		}
 
 		// 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_id         = _orb_topics[class_instance];
+		channel->node_id        = node_id;
+		channel->class_instance = class_instance;
 
 		channel->orb_advert = orb_advertise(channel->orb_id, report);
 		if (channel->orb_advert < 0) {
@@ -123,7 +125,7 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const
 			return;
 		}
 
-		log("channel %d class instance %d ok", channel->redunancy_channel_id, channel->class_instance);
+		log("channel %d class instance %d ok", channel->node_id, channel->class_instance);
 	}
 	assert(channel != nullptr);
 
@@ -134,9 +136,23 @@ 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) {
+		if (_channels[i].node_id >= 0) {
 			out += 1;
 		}
 	}
 	return out;
 }
+
+void UavcanCDevSensorBridgeBase::print_status() const
+{
+	printf("devname: %s\n", _class_devname);
+
+	for (unsigned i = 0; i < _max_channels; i++) {
+		if (_channels[i].node_id >= 0) {
+			printf("channel %d: node id %d --> class instance %d\n",
+			       i, _channels[i].node_id, _channels[i].class_instance);
+		} else {
+			printf("channel %d: empty\n", i);
+		}
+	}
+}
diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp
index a13d0ab35..1316f7ecc 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.hpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -69,6 +69,11 @@ public:
 	virtual unsigned get_num_redundant_channels() const = 0;
 
 	/**
+	 * Prints current status in a human readable format to stdout.
+	 */
+	virtual void print_status() 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.
@@ -84,7 +89,7 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD
 {
 	struct Channel
 	{
-		int redunancy_channel_id = -1;
+		int node_id              = -1;
 		orb_id_t orb_id          = nullptr;
 		orb_advert_t orb_advert  = -1;
 		int class_instance       = -1;
@@ -112,13 +117,15 @@ protected:
 	/**
 	 * 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
+	 * @param node_id Sensor's Node ID
+	 * @param report  Pointer to ORB message object
 	 */
-	void publish(const int redundancy_channel_id, const void *report);
+	void publish(const int node_id, const void *report);
 
 public:
 	virtual ~UavcanCDevSensorBridgeBase();
 
 	unsigned get_num_redundant_channels() const override;
+
+	void print_status() const override;
 };
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 482fec1e0..95c6ba13e 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -548,14 +548,16 @@ UavcanNode::print_info()
 	(void)pthread_mutex_lock(&_node_mutex);
 
 	// ESC mixer status
-	warnx("ESC actuators control groups: sub: %u / req: %u / fds: %u",
-	      (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
-	warnx("ESC mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
+	printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
+	       (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
+	printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
 
 	// Sensor bridges
 	auto br = _sensor_bridges.getHead();
 	while (br != nullptr) {
-		warnx("Sensor '%s': channels: %u", br->get_name(), br->get_num_redundant_channels());
+		printf("Sensor '%s':\n", br->get_name());
+		br->print_status();
+		printf("\n");
 		br = br->getSibling();
 	}