Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: use a 1s deadline for packets

Browse Source 
this fixes an issue with early discard of packets on MCUs with small
number of transmit slots and higher packet send count
This commit is contained in:
Andrew Tridgell 2023-12-16 08:44:25 +11:00 committed by Peter Barker
parent 23e67f7b53
commit cd72dcb73f
3 changed files with 126 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Tools/AP_Periph/AP_Periph.h
View File

@ -424,8 +424,16 @@ public:
uint16_t data_type_id, uint16_t data_type_id,
uint8_t priority, uint8_t priority,
const void* payload, const void* payload,
uint16_t payload_len); uint16_t payload_len,
uint8_t iface_mask=0);
bool canard_respond(CanardInstance* canard_instance,
CanardRxTransfer* transfer,
uint64_t data_type_signature,
uint16_t data_type_id,
const uint8_t *payload,
uint16_t payload_len);
void onTransferReceived(CanardInstance* canard_instance, void onTransferReceived(CanardInstance* canard_instance,
CanardRxTransfer* transfer); CanardRxTransfer* transfer);
bool shouldAcceptTransfer(const CanardInstance* canard_instance, bool shouldAcceptTransfer(const CanardInstance* canard_instance,
@ -473,7 +481,7 @@ public:
#if HAL_PERIPH_CAN_MIRROR #if HAL_PERIPH_CAN_MIRROR
void processMirror(void); void processMirror(void);
#endif // HAL_PERIPH_CAN_MIRROR #endif // HAL_PERIPH_CAN_MIRROR
void cleanup_stale_transactions(uint64_t &timestamp_usec); void cleanup_stale_transactions(uint64_t timestamp_usec);
void update_rx_protocol_stats(int16_t res); void update_rx_protocol_stats(int16_t res);
void node_status_send(void); void node_status_send(void);
bool can_do_dna(); bool can_do_dna();

208
Tools/AP_Periph/can.cpp
View File

@ -62,6 +62,9 @@ extern AP_Periph_FW periph;
#endif #endif
#endif #endif
// timeout all frames at 1s
#define CAN_FRAME_TIMEOUT 1000000ULL
#define DEBUG_PKTS 0 #define DEBUG_PKTS 0
#if HAL_PERIPH_CAN_MIRROR #if HAL_PERIPH_CAN_MIRROR
@ -207,25 +210,12 @@ void AP_Periph_FW::handle_get_node_info(CanardInstance* canard_instance,
uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, !canfdout()); uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, !canfdout());
const int16_t resp_res = canardRequestOrRespond(canard_instance, canard_respond(canard_instance,
transfer->source_node_id, transfer,
UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE,
UAVCAN_PROTOCOL_GETNODEINFO_ID, UAVCAN_PROTOCOL_GETNODEINFO_ID,
&transfer->transfer_id, &buffer[0],
transfer->priority, total_size);
CanardResponse,
&buffer[0],
total_size
#if CANARD_MULTI_IFACE
, IFACE_ALL
#endif
#if HAL_CANFD_SUPPORTED
, canfdout()
#endif
);
if (resp_res <= 0) {
printf("Could not respond to GetNodeInfo: %d\n", resp_res);
}
} }
/* /*
@ -316,23 +306,12 @@ void AP_Periph_FW::handle_param_getset(CanardInstance* canard_instance, CanardRx
uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE] {}; uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE] {};
uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer, !canfdout()); uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer, !canfdout());
canardRequestOrRespond(canard_instance, canard_respond(canard_instance,
transfer->source_node_id, transfer,
UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE, UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE,
UAVCAN_PROTOCOL_PARAM_GETSET_ID, UAVCAN_PROTOCOL_PARAM_GETSET_ID,
&transfer->transfer_id, &buffer[0],
transfer->priority, total_size);
CanardResponse,
&buffer[0],
total_size
#if CANARD_MULTI_IFACE
, IFACE_ALL
#endif
#if HAL_CANFD_SUPPORTED
,canfdout()
#endif
);
} }
/* /*
@ -376,22 +355,12 @@ void AP_Periph_FW::handle_param_executeopcode(CanardInstance* canard_instance, C
uint8_t buffer[UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE] {}; uint8_t buffer[UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE] {};
uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer, !canfdout()); uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer, !canfdout());
canardRequestOrRespond(canard_instance, canard_respond(canard_instance,
transfer->source_node_id, transfer,
UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE, UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE,
UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID, UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID,
&transfer->transfer_id, &buffer[0],
transfer->priority, total_size);
CanardResponse,
&buffer[0],
total_size
#if CANARD_MULTI_IFACE
, IFACE_ALL
#endif
#if HAL_CANFD_SUPPORTED
,canfdout()
#endif
);
} }
void AP_Periph_FW::handle_begin_firmware_update(CanardInstance* canard_instance, CanardRxTransfer* transfer) void AP_Periph_FW::handle_begin_firmware_update(CanardInstance* canard_instance, CanardRxTransfer* transfer)
@ -419,22 +388,12 @@ void AP_Periph_FW::handle_begin_firmware_update(CanardInstance* canard_instance,
reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK; reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK;
uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer, !canfdout()); uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer, !canfdout());
canardRequestOrRespond(canard_instance, canard_respond(canard_instance,
transfer->source_node_id, transfer,
UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE,
UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID,
&transfer->transfer_id, &buffer[0],
transfer->priority, total_size);
CanardResponse,
&buffer[0],
total_size
#if CANARD_MULTI_IFACE
,IFACE_ALL
#endif
#if HAL_CANFD_SUPPORTED
,canfdout()
#endif
);
uint8_t count = 50; uint8_t count = 50;
while (count--) { while (count--) {
processTx(); processTx();
@ -1015,7 +974,7 @@ static bool shouldAcceptTransfer_trampoline(const CanardInstance* canard_instanc
return fw->shouldAcceptTransfer(canard_instance, out_data_type_signature, data_type_id, transfer_type, source_node_id); return fw->shouldAcceptTransfer(canard_instance, out_data_type_signature, data_type_id, transfer_type, source_node_id);
} }
void AP_Periph_FW::cleanup_stale_transactions(uint64_t &timestamp_usec) void AP_Periph_FW::cleanup_stale_transactions(uint64_t timestamp_usec)
{ {
canardCleanupStaleTransfers(&dronecan.canard, timestamp_usec); canardCleanupStaleTransfers(&dronecan.canard, timestamp_usec);
} }
@ -1058,9 +1017,11 @@ bool AP_Periph_FW::canard_broadcast(uint64_t data_type_signature,
uint16_t data_type_id, uint16_t data_type_id,
uint8_t priority, uint8_t priority,
const void* payload, const void* payload,
uint16_t payload_len) uint16_t payload_len,
uint8_t iface_mask)
{ {
if (canardGetLocalNodeID(&dronecan.canard) == CANARD_BROADCAST_NODE_ID) { const bool is_dna = data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID;
if (!is_dna && canardGetLocalNodeID(&dronecan.canard) == CANARD_BROADCAST_NODE_ID) {
return false; return false;
} }
@ -1069,20 +1030,24 @@ bool AP_Periph_FW::canard_broadcast(uint64_t data_type_signature,
return false; return false;
} }
const int16_t res = canardBroadcast(&dronecan.canard, // create transfer object
data_type_signature, CanardTxTransfer transfer_object = {
data_type_id, .transfer_type = CanardTransferTypeBroadcast,
tid_ptr, .data_type_signature = data_type_signature,
priority, .data_type_id = data_type_id,
payload, .inout_transfer_id = tid_ptr,
payload_len .priority = priority,
.payload = (uint8_t*)payload,
.payload_len = payload_len,
#if CANARD_ENABLE_CANFD
.canfd = is_dna? false : canfdout(),
#endif
.deadline_usec = AP_HAL::micros64()+CAN_FRAME_TIMEOUT,
#if CANARD_MULTI_IFACE #if CANARD_MULTI_IFACE
, IFACE_ALL // send over all ifaces .iface_mask = iface_mask==0 ? uint8_t(IFACE_ALL) : iface_mask,
#endif #endif
#if HAL_CANFD_SUPPORTED };
, canfdout() const int16_t res = canardBroadcastObj(&dronecan.canard, &transfer_object);
#endif
);
#if DEBUG_PKTS #if DEBUG_PKTS
if (res < 0) { if (res < 0) {
@ -1099,6 +1064,50 @@ bool AP_Periph_FW::canard_broadcast(uint64_t data_type_signature,
return res > 0; return res > 0;
} }
/*
send a response
*/
bool AP_Periph_FW::canard_respond(CanardInstance* canard_instance,
CanardRxTransfer *transfer,
uint64_t data_type_signature,
uint16_t data_type_id,
const uint8_t *payload,
uint16_t payload_len)
{
CanardTxTransfer transfer_object = {
.transfer_type = CanardTransferTypeResponse,
.data_type_signature = data_type_signature,
.data_type_id = data_type_id,
.inout_transfer_id = &transfer->transfer_id,
.priority = transfer->priority,
.payload = payload,
.payload_len = payload_len,
#if CANARD_ENABLE_CANFD
.canfd = canfdout(),
#endif
.deadline_usec = AP_HAL::micros64()+CAN_FRAME_TIMEOUT,
#if CANARD_MULTI_IFACE
.iface_mask = IFACE_ALL,
#endif
};
const auto res = canardRequestOrRespondObj(canard_instance,
transfer->source_node_id,
&transfer_object);
#if DEBUG_PKTS
if (res < 0) {
can_printf("Tx error %d\n", res);
}
#endif
#if HAL_ENABLE_SENDING_STATS
if (res <= 0) {
protocol_stats.tx_errors++;
} else {
protocol_stats.tx_frames += res;
}
#endif
return res > 0;
}
void AP_Periph_FW::processTx(void) void AP_Periph_FW::processTx(void)
{ {
for (CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&dronecan.canard)) != NULL;) { for (CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&dronecan.canard)) != NULL;) {
@ -1151,15 +1160,17 @@ void AP_Periph_FW::processTx(void)
canardPopTxQueue(&dronecan.canard); canardPopTxQueue(&dronecan.canard);
dronecan.tx_fail_count = 0; dronecan.tx_fail_count = 0;
} else { } else {
// just exit and try again later. If we fail 8 times in a row // exit and try again later. If we fail 8 times in a row
// then start discarding to prevent the pool filling up // then cleanup any stale transfers to keep the queue from
// filling
if (dronecan.tx_fail_count < 8) { if (dronecan.tx_fail_count < 8) {
dronecan.tx_fail_count++; dronecan.tx_fail_count++;
} else { } else {
#if HAL_ENABLE_SENDING_STATS #if HAL_ENABLE_SENDING_STATS
protocol_stats.tx_errors++; protocol_stats.tx_errors++;
#endif #endif
canardPopTxQueue(&dronecan.canard); dronecan.tx_fail_count = 0;
cleanup_stale_transactions(now_us);
} }
break; break;
} }
@ -1490,8 +1501,6 @@ bool AP_Periph_FW::can_do_dna()
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
static uint8_t node_id_allocation_transfer_id = 0;
if (AP_Periph_FW::no_iface_finished_dna) { if (AP_Periph_FW::no_iface_finished_dna) {
printf("Waiting for dynamic node ID allocation %x... (pool %u)\n", IFACE_ALL, pool_peak_percent()); printf("Waiting for dynamic node ID allocation %x... (pool %u)\n", IFACE_ALL, pool_peak_percent());
} }
@ -1525,24 +1534,11 @@ bool AP_Periph_FW::can_do_dna()
memmove(&allocation_request[1], &my_unique_id[dronecan.node_id_allocation_unique_id_offset], uid_size); memmove(&allocation_request[1], &my_unique_id[dronecan.node_id_allocation_unique_id_offset], uid_size);
// Broadcasting the request // Broadcasting the request
const int16_t bcast_res = canardBroadcast(&dronecan.canard, canard_broadcast(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE,
UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, CANARD_TRANSFER_PRIORITY_LOW,
&node_id_allocation_transfer_id, &allocation_request[0],
CANARD_TRANSFER_PRIORITY_LOW, (uint16_t) (uid_size + 1));
&allocation_request[0],
(uint16_t) (uid_size + 1)
#if CANARD_MULTI_IFACE
,(1U << dronecan.dna_interface)
#endif
#if HAL_CANFD_SUPPORTED
// always send allocation request as non-FD
,false
#endif
);
if (bcast_res < 0) {
printf("Could not broadcast ID allocation req; error %d\n", bcast_res);
}
// Preparing for timeout; if response is received, this value will be updated from the callback. // Preparing for timeout; if response is received, this value will be updated from the callback.
dronecan.node_id_allocation_unique_id_offset = 0; dronecan.node_id_allocation_unique_id_offset = 0;

44
Tools/AP_Periph/gps.cpp
View File

@ -163,10 +163,10 @@ void AP_Periph_FW::can_gps_update(void)
uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer, !canfdout()); uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer, !canfdout());
canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE, canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE,
UAVCAN_EQUIPMENT_GNSS_FIX2_ID, UAVCAN_EQUIPMENT_GNSS_FIX2_ID,
CANARD_TRANSFER_PRIORITY_LOW, CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0], &buffer[0],
total_size); total_size);
} }
/* /*
@ -258,34 +258,18 @@ void AP_Periph_FW::send_moving_baseline_msg()
uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {}; uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {};
const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !canfdout()); const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !canfdout());
#if HAL_NUM_CAN_IFACES >= 2 uint8_t iface_mask = 0;
#if HAL_NUM_CAN_IFACES >= 2 && CANARD_MULTI_IFACE
if (gps_mb_can_port != -1 && (gps_mb_can_port < HAL_NUM_CAN_IFACES)) { if (gps_mb_can_port != -1 && (gps_mb_can_port < HAL_NUM_CAN_IFACES)) {
uint8_t *tid_ptr = get_tid_ptr(MAKE_TRANSFER_DESCRIPTOR(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, 0, CANARD_BROADCAST_NODE_ID)); iface_mask = 1U<<gps_mb_can_port;
canardBroadcast(&dronecan.canard,
ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE,
ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID,
tid_ptr,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size
#if CANARD_MULTI_IFACE
,(1U<<gps_mb_can_port)
#endif
#if HAL_CANFD_SUPPORTED
,canfdout()
#endif
);
} else
#endif
{
// we use MEDIUM priority on this data as we need to get all
// the data through for RTK moving baseline yaw to work
canard_broadcast(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE,
ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID,
CANARD_TRANSFER_PRIORITY_MEDIUM,
&buffer[0],
total_size);
} }
#endif
canard_broadcast(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE,
ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size,
iface_mask);
gps.clear_RTCMV3(); gps.clear_RTCMV3();
#endif // GPS_MOVING_BASELINE #endif // GPS_MOVING_BASELINE
} }