uavcan/uavcannode: bridge LogMessage and PX4 ORB_ID(log_message_s)

src/drivers/uavcannode/UavcanNode.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2022 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
@@ -405,6 +405,8 @@ void UavcanNode::Run()
 			_task_should_exit.store(true);
 		}
 
+		_node.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+
 		_node.setModeOperational();
 
 		_initialized = true;
@@ -428,6 +430,61 @@ void UavcanNode::Run()
 		publisher->BroadcastAnyUpdates();
 	}
 
+	if (_log_message_sub.updated()) {
+		log_message_s log_message;
+
+		if (_log_message_sub.copy(&log_message)) {
+			char source[31] {};
+			char text[90] {};
+
+			bool text_copied = false;
+
+			if (log_message.text[0] == '[') {
+				// find closing bracket ]
+				for (size_t i = 0; i < strlen(log_message.text); i++) {
+					if (log_message.text[i] == ']') {
+						// copy [MODULE_NAME] to source
+						memcpy(source, &log_message.text[1], i - 1);
+						// copy remaining text (skipping space after [])
+						memcpy(text, &log_message.text[i + 2], math::min(sizeof(log_message.text) - (i + 2), sizeof(text)));
+
+						text_copied = true;
+					}
+				}
+			}
+
+			if (!text_copied) {
+				memcpy(text, log_message.text, sizeof(text));
+			}
+
+			switch (log_message.severity) {
+			case 7: // debug
+				_node.getLogger().logDebug(source, text);
+				break;
+
+			case 6: // info
+				_node.getLogger().logInfo(source, text);
+				break;
+
+			case 4: // warn
+				_node.getLogger().logWarning(source, text);
+				break;
+
+			case 3: // error
+				_node.getLogger().logError(source, text);
+				break;
+
+			case 0: // panic
+				_node.getLogger().logError(source, text);
+				break;
+
+			default:
+				_node.getLogger().logInfo(source, text);
+				break;
+			}
+		}
+	}
+
 	_node.spinOnce();
 
 	// This is done only once to signify the node has run 30 seconds

src/drivers/uavcannode/UavcanNode.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2022 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
@@ -67,6 +67,7 @@
 
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/log_message.h>
 #include <uORB/topics/parameter_update.h>
 
 #include "Publishers/UavcanPublisherBase.hpp"
@@ -170,6 +171,7 @@ private:
 	IntrusiveSortedList<UavcanSubscriberBase *> _subscriber_list;
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::Subscription _log_message_sub{ORB_ID(log_message)};
 
 	UavcanNodeParamManager _param_manager;
 	uavcan::ParamServer _param_server;