From a0e541893cb430cd12a6a275899b0c5b2553ccdb Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Fri, 7 Jul 2023 17:20:37 +1000 Subject: [PATCH] AP_DroneCAN: add support for sending CAN and Protocol Stats --- libraries/AP_DroneCAN/AP_Canard_iface.cpp | 99 ++++++++++++++++++----- libraries/AP_DroneCAN/AP_Canard_iface.h | 4 + libraries/AP_DroneCAN/AP_DroneCAN.cpp | 44 +++++++++- libraries/AP_DroneCAN/AP_DroneCAN.h | 3 + 4 files changed, 129 insertions(+), 21 deletions(-) diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.cpp b/libraries/AP_DroneCAN/AP_Canard_iface.cpp index eecce54a10..57b2d7f234 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.cpp +++ b/libraries/AP_DroneCAN/AP_Canard_iface.cpp @@ -96,13 +96,18 @@ bool CanardInterface::broadcast(const Canard::Transfer &bcast_transfer) { #endif }; // do canard broadcast - bool success = canardBroadcastObj(&canard, &tx_transfer) > 0; + int16_t ret = canardBroadcastObj(&canard, &tx_transfer); #if AP_TEST_DRONECAN_DRIVERS if (this == &test_iface) { test_iface_sem.give(); } #endif - return success; + if (ret <= 0) { + protocol_stats.tx_errors++; + } else { + protocol_stats.tx_frames += ret; + } + return ret > 0; } bool CanardInterface::request(uint8_t destination_node_id, const Canard::Transfer &req_transfer) { @@ -128,7 +133,13 @@ bool CanardInterface::request(uint8_t destination_node_id, const Canard::Transfe #endif }; // do canard request - return canardRequestOrRespondObj(&canard, destination_node_id, &tx_transfer) > 0; + int16_t ret = canardRequestOrRespondObj(&canard, destination_node_id, &tx_transfer); + if (ret <= 0) { + protocol_stats.tx_errors++; + } else { + protocol_stats.tx_frames += ret; + } + return ret > 0; } bool CanardInterface::respond(uint8_t destination_node_id, const Canard::Transfer &res_transfer) { @@ -154,7 +165,13 @@ bool CanardInterface::respond(uint8_t destination_node_id, const Canard::Transfe #endif }; // do canard respond - return canardRequestOrRespondObj(&canard, destination_node_id, &tx_transfer) > 0; + int16_t ret = canardRequestOrRespondObj(&canard, destination_node_id, &tx_transfer); + if (ret <= 0) { + protocol_stats.tx_errors++; + } else { + protocol_stats.tx_frames += ret; + } + return ret > 0; } void CanardInterface::onTransferReception(CanardInstance* ins, CanardRxTransfer* transfer) { @@ -244,6 +261,49 @@ void CanardInterface::processTx(bool raw_commands_only = false) { } +void CanardInterface::update_rx_protocol_stats(int16_t res) +{ + switch (res) { + case CANARD_OK: + protocol_stats.rx_frames++; + break; + case -CANARD_ERROR_OUT_OF_MEMORY: + protocol_stats.rx_error_oom++; + break; + case -CANARD_ERROR_INTERNAL: + protocol_stats.rx_error_internal++; + break; + case -CANARD_ERROR_RX_INCOMPATIBLE_PACKET: + protocol_stats.rx_ignored_not_wanted++; + break; + case -CANARD_ERROR_RX_WRONG_ADDRESS: + protocol_stats.rx_ignored_wrong_address++; + break; + case -CANARD_ERROR_RX_NOT_WANTED: + protocol_stats.rx_ignored_not_wanted++; + break; + case -CANARD_ERROR_RX_MISSED_START: + protocol_stats.rx_error_missed_start++; + break; + case -CANARD_ERROR_RX_WRONG_TOGGLE: + protocol_stats.rx_error_wrong_toggle++; + break; + case -CANARD_ERROR_RX_UNEXPECTED_TID: + protocol_stats.rx_ignored_unexpected_tid++; + break; + case -CANARD_ERROR_RX_SHORT_FRAME: + protocol_stats.rx_error_short_frame++; + break; + case -CANARD_ERROR_RX_BAD_CRC: + protocol_stats.rx_error_bad_crc++; + break; + default: + // mark all other errors as internal + protocol_stats.rx_error_internal++; + break; + } +} + void CanardInterface::processRx() { AP_HAL::CANFrame rxmsg; for (uint8_t i=0; iprintf("DTID: %u\n", extractDataType(rx_frame.id)); - // hal.console->printf("Rx %d, IF%d %lx: ", res, i, rx_frame.id); - if (res < 0 && - res != -CANARD_ERROR_RX_NOT_WANTED && - res != -CANARD_ERROR_RX_WRONG_ADDRESS) { - hal.console->printf("Rx error %d, IF%d %lx: \n", res, i, rx_frame.id); - // for (uint8_t index = 0; index < rx_frame.data_len; index++) { - // hal.console->printf("%02x", rx_frame.data[index]); - // } - // hal.console->printf("\n"); + const int16_t res = canardHandleRxFrame(&canard, &rx_frame, timestamp); + if (res == -CANARD_ERROR_RX_MISSED_START) { + // this might remaining frames from a message that we don't accept, so check + uint64_t dummy_signature; + if (shouldAcceptTransfer(&canard, + &dummy_signature, + extractDataType(rx_frame.id), + extractTransferType(rx_frame.id), + 1)) { // doesn't matter what we pass here + update_rx_protocol_stats(res); + } else { + protocol_stats.rx_ignored_not_wanted++; + } + } else { + update_rx_protocol_stats(res); } -#endif } } } diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.h b/libraries/AP_DroneCAN/AP_Canard_iface.h index 74df8581d3..aa7533e691 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.h +++ b/libraries/AP_DroneCAN/AP_Canard_iface.h @@ -2,6 +2,7 @@ #include #if HAL_ENABLE_DRONECAN_DRIVERS #include +#include class AP_DroneCAN; class CanardInterface : public Canard::Interface { @@ -52,6 +53,8 @@ public: static void processTestRx(); #endif + void update_rx_protocol_stats(int16_t res); + uint8_t get_node_id() const override { return canard.node_id; } private: CanardInstance canard; @@ -66,5 +69,6 @@ private: HAL_Semaphore _sem_tx; HAL_Semaphore _sem_rx; CanardTxTransfer tx_transfer; + dronecan_protocol_Stats protocol_stats; }; #endif // HAL_ENABLE_DRONECAN_DRIVERS \ No newline at end of file diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 82a86e8b99..d3921e4257 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -113,7 +113,7 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = { // @Param: OPTION // @DisplayName: DroneCAN options // @Description: Option flags - // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo + // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo,7:HobbyWingESC,8:EnableStats // @User: Advanced AP_GROUPINFO("OPTION", 5, AP_DroneCAN, _options, 0), @@ -323,6 +323,12 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters) node_status.set_priority(CANARD_TRANSFER_PRIORITY_LOWEST); node_status.set_timeout_ms(1000); + protocol_stats.set_priority(CANARD_TRANSFER_PRIORITY_LOWEST); + protocol_stats.set_timeout_ms(3000); + + can_stats.set_priority(CANARD_TRANSFER_PRIORITY_LOWEST); + can_stats.set_timeout_ms(3000); + rgb_led.set_timeout_ms(20); rgb_led.set_priority(CANARD_TRANSFER_PRIORITY_LOW); @@ -481,6 +487,42 @@ void AP_DroneCAN::send_node_status(void) _node_status_last_send_ms = now; node_status_msg.uptime_sec = now / 1000; node_status.broadcast(node_status_msg); + + if (option_is_set(Options::ENABLE_STATS)) { + // also send protocol and can stats + protocol_stats.broadcast(canard_iface.protocol_stats); + + // get can stats + for (uint8_t i=0; iget_statistics(); + if (bus_stats == nullptr) { + continue; + } + dronecan_protocol_CanStats can_stats_msg; + can_stats_msg.interface = i; + can_stats_msg.tx_requests = bus_stats->tx_requests; + can_stats_msg.tx_rejected = bus_stats->tx_rejected; + can_stats_msg.tx_overflow = bus_stats->tx_overflow; + can_stats_msg.tx_success = bus_stats->tx_success; + can_stats_msg.tx_timedout = bus_stats->tx_timedout; + can_stats_msg.tx_abort = bus_stats->tx_abort; + can_stats_msg.rx_received = bus_stats->rx_received; + can_stats_msg.rx_overflow = bus_stats->rx_overflow; + can_stats_msg.rx_errors = bus_stats->rx_errors; + can_stats_msg.busoff_errors = bus_stats->num_busoff_err; + can_stats.broadcast(can_stats_msg); + } + } } void AP_DroneCAN::handle_node_info_request(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoRequest& req) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index 4d42e81668..6c267fc579 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -114,6 +114,7 @@ public: SEND_GNSS = (1U<<5), USE_HIMARK_SERVO = (1U<<6), USE_HOBBYWING_ESC = (1U<<7), + ENABLE_STATS = (1U<<8), }; // check if a option is set @@ -235,6 +236,8 @@ private: CanardInterface canard_iface; Canard::Publisher node_status{canard_iface}; + Canard::Publisher can_stats{canard_iface}; + Canard::Publisher protocol_stats{canard_iface}; Canard::Publisher act_out_array{canard_iface}; Canard::Publisher esc_raw{canard_iface}; Canard::Publisher safety_state{canard_iface};