Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_DroneCAN: add support for sending CAN and Protocol Stats

This commit is contained in:
bugobliterator 2023-07-07 17:20:37 +10:00 committed by Andrew Tridgell
parent 654d2916da
commit a0e541893c
4 changed files with 129 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
libraries/AP_DroneCAN/AP_Canard_iface.cpp
View File

@ -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; i<num_ifaces; i++) {
@ -277,23 +337,22 @@ void CanardInterface::processRx() {
{
WITH_SEMAPHORE(_sem_rx);
#if DEBUG_PKTS
const int16_t res =
#endif
canardHandleRxFrame(&canard, &rx_frame, timestamp);
#if DEBUG_PKTS
// hal.console->printf("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
}
}
}

4
libraries/AP_DroneCAN/AP_Canard_iface.h
View File

@ -2,6 +2,7 @@
#include <AP_HAL/AP_HAL.h>
#if HAL_ENABLE_DRONECAN_DRIVERS
#include <canard/interface.h>
#include <dronecan_msgs.h>
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

44
libraries/AP_DroneCAN/AP_DroneCAN.cpp
View File

@ -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; i<canard_iface.num_ifaces; i++) {
if (canard_iface.ifaces[i] == nullptr) {
continue;
}
auto* iface = hal.can[0];
for (uint8_t j=0; j<HAL_NUM_CAN_IFACES; j++) {
if (hal.can[j] == canard_iface.ifaces[i]) {
iface = hal.can[j];
break;
}
}
auto* bus_stats = iface->get_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)

3
libraries/AP_DroneCAN/AP_DroneCAN.h
View File

@ -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<uavcan_protocol_NodeStatus> node_status{canard_iface};
Canard::Publisher<dronecan_protocol_CanStats> can_stats{canard_iface};
Canard::Publisher<dronecan_protocol_Stats> protocol_stats{canard_iface};
Canard::Publisher<uavcan_equipment_actuator_ArrayCommand> act_out_array{canard_iface};
Canard::Publisher<uavcan_equipment_esc_RawCommand> esc_raw{canard_iface};
Canard::Publisher<ardupilot_indication_SafetyState> safety_state{canard_iface};