diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 74cb613942..07ba27b3a3 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/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 diff --git a/src/drivers/uavcan/logmessage.hpp b/src/drivers/uavcan/logmessage.hpp new file mode 100644 index 0000000000..53f2aacb6a --- /dev/null +++ b/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 + +#include +#include + +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 &) > + LogMessageCbBinder; + + void logmessage_sub_cb(const uavcan::ReceivedDataStructure &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 _sub_logmessage; +}; diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index fb094d0ed6..42724c6212 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/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) { diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 57b78aeaf0..f8524ba96c 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/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; diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 4c785fe787..d6c3fa43b2 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/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 diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp index 3bee1eb276..06d44bc2a2 100644 --- a/src/drivers/uavcannode/UavcanNode.hpp +++ b/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 #include +#include #include #include "Publishers/UavcanPublisherBase.hpp" @@ -170,6 +171,7 @@ private: IntrusiveSortedList _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;