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

src/drivers/uavcan/CMakeLists.txt

@@ -127,6 +127,8 @@ px4_add_module(
 	MODULE drivers__uavcan
 	MAIN uavcan
 	STACK_MAIN 4096
+	COMPILE_FLAGS
+		-Wno-format-security # logmessage.hpp
 	INCLUDES
 		${DSDLC_OUTPUT}
 		${LIBUAVCAN_DIR}/libuavcan/include

src/drivers/uavcan/logmessage.hpp

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/log.h>
+
+#include <uavcan/uavcan.hpp>
+#include <uavcan/protocol/debug/LogMessage.hpp>
+
+class UavcanLogMessage
+{
+public:
+	UavcanLogMessage(uavcan::INode &node) : _sub_logmessage(node) {}
+	~UavcanLogMessage() = default;
+
+	int init()
+	{
+		int res = _sub_logmessage.start(LogMessageCbBinder(this, &UavcanLogMessage::logmessage_sub_cb));
+
+		if (res < 0) {
+			PX4_ERR("LogMessage sub failed %i", res);
+			return res;
+		}
+
+		return 0;
+	}
+
+private:
+	typedef uavcan::MethodBinder < UavcanLogMessage *,
+		void (UavcanLogMessage::*)(const uavcan::ReceivedDataStructure<uavcan::protocol::debug::LogMessage> &) >
+		LogMessageCbBinder;
+
+	void logmessage_sub_cb(const uavcan::ReceivedDataStructure<uavcan::protocol::debug::LogMessage> &msg)
+	{
+		int px4_level = _PX4_LOG_LEVEL_INFO;
+
+		switch (msg.level.value) {
+		case uavcan::protocol::debug::LogLevel::DEBUG:
+			px4_level = _PX4_LOG_LEVEL_DEBUG;
+			break;
+
+		case uavcan::protocol::debug::LogLevel::INFO:
+			px4_level = _PX4_LOG_LEVEL_INFO;
+			break;
+
+		case uavcan::protocol::debug::LogLevel::WARNING:
+			px4_level = _PX4_LOG_LEVEL_WARN;
+			break;
+
+		case uavcan::protocol::debug::LogLevel::ERROR:
+			px4_level = _PX4_LOG_LEVEL_ERROR;
+			break;
+		}
+
+		char module_name_buffer[80];
+		snprintf(module_name_buffer, sizeof(module_name_buffer), "uavcan:%d:%s", msg.getSrcNodeID().get(), msg.source.c_str());
+		px4_log_modulename(px4_level, module_name_buffer, msg.text.c_str());
+	}
+
+	uavcan::Subscriber<uavcan::protocol::debug::LogMessage, LogMessageCbBinder> _sub_logmessage;
+};

src/drivers/uavcan/uavcan_main.cpp

@@ -86,6 +86,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	_servo_controller(_node),
 	_hardpoint_controller(_node),
 	_safety_state_controller(_node),
+	_log_message_controller(_node),
 	_rgbled_controller(_node),
 	_time_sync_master(_node),
 	_time_sync_slave(_node),
@@ -532,6 +533,12 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 		return ret;
 	}
 
+	ret = _log_message_controller.init();
+
+	if (ret < 0) {
+		return ret;
+	}
+
 	ret = _rgbled_controller.init();
 
 	if (ret < 0) {

src/drivers/uavcan/uavcan_main.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-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
@@ -51,6 +51,7 @@
 #include "actuators/servo.hpp"
 #include "allocator.hpp"
 #include "beep.hpp"
+#include "logmessage.hpp"
 #include "rgbled.hpp"
 #include "safety_state.hpp"
 #include "sensors/sensor_bridge.hpp"
@@ -233,6 +234,7 @@ private:
 	UavcanMixingInterfaceServo 	_mixing_interface_servo{_node_mutex, _servo_controller};
 	UavcanHardpointController	_hardpoint_controller;
 	UavcanSafetyState         	_safety_state_controller;
+	UavcanLogMessage                _log_message_controller;
 	UavcanRGBController             _rgbled_controller;
 
 	uavcan::GlobalTimeSyncMaster	_time_sync_master;

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;