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;