From 51a545f0b9eb7c61e24811e81f60167982568ef6 Mon Sep 17 00:00:00 2001 From: Marius Eskedal Date: Tue, 24 Oct 2023 10:09:22 +0200 Subject: [PATCH] Add optional serial print Added serial printing macro that handles the optional printing parameter before printing to simplify usage. Also applied this for serial debugging outputs in the receive loop of CAN. --- RemoteIDModule/DroneCAN.cpp | 13 +++++++------ RemoteIDModule/log.hpp | 9 +++++++++ RemoteIDModule/mavlink.cpp | 29 ++++++++--------------------- RemoteIDModule/parameters.h | 2 +- 4 files changed, 25 insertions(+), 28 deletions(-) create mode 100644 RemoteIDModule/log.hpp diff --git a/RemoteIDModule/DroneCAN.cpp b/RemoteIDModule/DroneCAN.cpp index 63c82c3..798dc1c 100644 --- a/RemoteIDModule/DroneCAN.cpp +++ b/RemoteIDModule/DroneCAN.cpp @@ -4,6 +4,7 @@ */ #include #include "board_config.h" +#include "log.hpp" #include "version.h" #include #include "DroneCAN.h" @@ -153,28 +154,28 @@ void DroneCAN::onTransferReceived(CanardInstance* ins, handle_get_node_info(ins, transfer); break; case UAVCAN_PROTOCOL_RESTARTNODE_ID: - Serial.printf("DroneCAN: restartNode\n"); + serial_log("DroneCAN: restartNode\n"); delay(20); esp_restart(); break; case DRONECAN_REMOTEID_BASICID_ID: - Serial.printf("DroneCAN: got BasicID\n"); + serial_log("DroneCAN: got BasicID\n"); handle_BasicID(transfer); break; case DRONECAN_REMOTEID_LOCATION_ID: - Serial.printf("DroneCAN: got Location\n"); + serial_log("DroneCAN: got Location\n"); handle_Location(transfer); break; case DRONECAN_REMOTEID_SELFID_ID: - Serial.printf("DroneCAN: got SelfID\n"); + serial_log("DroneCAN: got SelfID\n"); handle_SelfID(transfer); break; case DRONECAN_REMOTEID_SYSTEM_ID: - Serial.printf("DroneCAN: got System\n"); + serial_log("DroneCAN: got System\n"); handle_System(transfer); break; case DRONECAN_REMOTEID_OPERATORID_ID: - Serial.printf("DroneCAN: got OperatorID\n"); + serial_log("DroneCAN: got OperatorID\n"); handle_OperatorID(transfer); break; case UAVCAN_PROTOCOL_PARAM_GETSET_ID: diff --git a/RemoteIDModule/log.hpp b/RemoteIDModule/log.hpp new file mode 100644 index 0000000..29c736f --- /dev/null +++ b/RemoteIDModule/log.hpp @@ -0,0 +1,9 @@ +#pragma once + +#include +#include + +#define serial_log(format, args...) \ + if (g.options & OPTIONS_PRINT_RID_DEBUG) { \ + Serial.printf(format, ## args); \ + } diff --git a/RemoteIDModule/mavlink.cpp b/RemoteIDModule/mavlink.cpp index 88086c0..b632647 100644 --- a/RemoteIDModule/mavlink.cpp +++ b/RemoteIDModule/mavlink.cpp @@ -2,6 +2,7 @@ mavlink class for handling OpenDroneID messages */ #include +#include "log.hpp" #include "mavlink.h" #include "board_config.h" #include "version.h" @@ -144,9 +145,7 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t & } case MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION: { mavlink_msg_open_drone_id_location_decode(&msg, &location); - if (g.options & OPTIONS_PRINT_RID_MAVLINK) { - Serial.printf("MAVLink: got Location\n"); - } + serial_log("MAVLink: got Location\n"); if (last_location_timestamp != location.timestamp) { //only update the timestamp if we receive information with a different timestamp last_location_ms = millis(); @@ -157,9 +156,7 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t & } case MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID: { mavlink_open_drone_id_basic_id_t basic_id_tmp; - if (g.options & OPTIONS_PRINT_RID_MAVLINK) { - Serial.printf("MAVLink: got BasicID\n"); - } + serial_log("MAVLink: got BasicID\n"); mavlink_msg_open_drone_id_basic_id_decode(&msg, &basic_id_tmp); if ((strlen((const char*) basic_id_tmp.uas_id) > 0) && (basic_id_tmp.id_type > 0) && (basic_id_tmp.id_type <= MAV_ODID_ID_TYPE_SPECIFIC_SESSION_ID)) { //only update if we receive valid data @@ -170,24 +167,18 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t & } case MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION: { mavlink_msg_open_drone_id_authentication_decode(&msg, &authentication); - if (g.options & OPTIONS_PRINT_RID_MAVLINK) { - Serial.printf("MAVLink: got Auth\n"); - } + serial_log("MAVLink: got Auth\n"); break; } case MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID: { mavlink_msg_open_drone_id_self_id_decode(&msg, &self_id); - if (g.options & OPTIONS_PRINT_RID_MAVLINK) { - Serial.printf("MAVLink: got SelfID\n"); - } + serial_log("MAVLink: got SelfID\n"); last_self_id_ms = now_ms; break; } case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM: { mavlink_msg_open_drone_id_system_decode(&msg, &system); - if (g.options & OPTIONS_PRINT_RID_MAVLINK) { - Serial.printf("MAVLink: got System\n"); - } + serial_log("MAVLink: got System\n"); if ((last_system_timestamp != system.timestamp) || (system.timestamp == 0)) { //only update the timestamp if we receive information with a different timestamp last_system_ms = millis(); @@ -196,9 +187,7 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t & break; } case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE: { - if (g.options & OPTIONS_PRINT_RID_MAVLINK) { - Serial.printf("MAVLink: got System update\n"); - } + serial_log("MAVLink: got System update\n"); mavlink_open_drone_id_system_update_t pkt_system_update; mavlink_msg_open_drone_id_system_update_decode(&msg, &pkt_system_update); system.operator_latitude = pkt_system_update.operator_latitude; @@ -219,9 +208,7 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t & } case MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID: { mavlink_msg_open_drone_id_operator_id_decode(&msg, &operator_id); - if (g.options & OPTIONS_PRINT_RID_MAVLINK) { - Serial.printf("MAVLink: got OperatorID\n"); - } + serial_log("MAVLink: got OperatorID\n"); last_operator_id_ms = now_ms; break; } diff --git a/RemoteIDModule/parameters.h b/RemoteIDModule/parameters.h index b537c97..71d2327 100644 --- a/RemoteIDModule/parameters.h +++ b/RemoteIDModule/parameters.h @@ -109,6 +109,6 @@ class Parameters { // bits for OPTIONS parameter #define OPTIONS_FORCE_ARM_OK (1U<<0) #define OPTIONS_DONT_SAVE_BASIC_ID_TO_PARAMETERS (1U<<1) -#define OPTIONS_PRINT_RID_MAVLINK (1U<<2) +#define OPTIONS_PRINT_RID_DEBUG (1U<<2) extern Parameters g;