Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: added logging of UAVCAN LogMessage messages

Browse Source 
this allows us to log internal errors from nodes into main flight
controller log
This commit is contained in:
Andrew Tridgell 2020-11-30 10:47:32 +11:00
parent b2885e3e32
commit 88b5523d47
2 changed files with 30 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -41,6 +41,7 @@
#include <ardupilot/indication/Button.hpp> #include <ardupilot/indication/Button.hpp>
#include <ardupilot/equipment/trafficmonitor/TrafficReport.hpp> #include <ardupilot/equipment/trafficmonitor/TrafficReport.hpp>
#include <uavcan/equipment/gnss/RTCMStream.hpp> #include <uavcan/equipment/gnss/RTCMStream.hpp>
#include <uavcan/protocol/debug/LogMessage.hpp>
#include <AP_Baro/AP_Baro_UAVCAN.h> #include <AP_Baro/AP_Baro_UAVCAN.h>
#include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h> #include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h>
@ -129,6 +130,10 @@ static uavcan::Subscriber<uavcan::equipment::actuator::Status, ActuatorStatusCb>
UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status); UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status);
static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
// handler DEBUG
UC_REGISTRY_BINDER(DebugCb, uavcan::protocol::debug::LogMessage);
static uavcan::Subscriber<uavcan::protocol::debug::LogMessage, DebugCb> *debug_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
AP_UAVCAN::esc_data AP_UAVCAN::_escs_data[]; AP_UAVCAN::esc_data AP_UAVCAN::_escs_data[];
HAL_Semaphore AP_UAVCAN::_telem_sem; HAL_Semaphore AP_UAVCAN::_telem_sem;
@ -302,6 +307,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
if (esc_status_listener[driver_index]) { if (esc_status_listener[driver_index]) {
esc_status_listener[driver_index]->start(ESCStatusCb(this, &handle_ESC_status)); esc_status_listener[driver_index]->start(ESCStatusCb(this, &handle_ESC_status));
} }
debug_listener[driver_index] = new uavcan::Subscriber<uavcan::protocol::debug::LogMessage, DebugCb>(*_node);
if (debug_listener[driver_index]) {
debug_listener[driver_index]->start(DebugCb(this, &handle_debug));
}
_led_conf.devices_count = 0; _led_conf.devices_count = 0;
if (enable_filters) { if (enable_filters) {
@ -871,4 +881,21 @@ bool AP_UAVCAN::is_esc_data_index_valid(const uint8_t index) {
return true; return true;
} }
/*
handle LogMessage debug
*/
void AP_UAVCAN::handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb)
{
#ifndef HAL_NO_LOGGING
const auto &msg = *cb.msg;
if (AP::can().get_log_level() != AP_CANManager::LOG_NONE) {
// log to onboard log and mavlink
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CAN[%u] %s", node_id, msg.text.c_str());
} else {
// only log to onboard log
AP::logger().Write_MessageF("CAN[%u] %s", node_id, msg.text.c_str());
}
#endif
}
#endif // HAL_NUM_CAN_IFACES #endif // HAL_NUM_CAN_IFACES

4
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -56,6 +56,7 @@ class ButtonCb;
class TrafficReportCb; class TrafficReportCb;
class ActuatorStatusCb; class ActuatorStatusCb;
class ESCStatusCb; class ESCStatusCb;
class DebugCb;
#if defined(__GNUC__) && (__GNUC__ > 8) #if defined(__GNUC__) && (__GNUC__ > 8)
#define DISABLE_W_CAST_FUNCTION_TYPE_PUSH \ #define DISABLE_W_CAST_FUNCTION_TYPE_PUSH \
@ -241,12 +242,13 @@ private:
// safety status send state // safety status send state
uint32_t _last_safety_state_ms; uint32_t _last_safety_state_ms;
// safety button handling // incoming button handling
static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb); static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb);
static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb); static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
static void handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb); static void handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb);
static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb); static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb);
static bool is_esc_data_index_valid(const uint8_t index); static bool is_esc_data_index_valid(const uint8_t index);
static void handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb);
}; };
#endif // #if HAL_ENABLE_LIBUAVCAN_DRIVERS #endif // #if HAL_ENABLE_LIBUAVCAN_DRIVERS