Skip to content

Commit

Permalink
AP_Periph: use a 1s deadline for packets
Browse files Browse the repository at this point in the history 
this fixes an issue with early discard of packets on MCUs with small
number of transmit slots and higher packet send count
  • Loading branch information
@tridge
tridge committed Dec 15, 2023
1 parent 97b6c85 commit be12f9e
Show file tree
Hide file tree
Showing 3 changed files with 126 additions and 138 deletions.
14 changes: 11 additions & 3 deletions Tools/AP_Periph/AP_Periph.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -416,8 +416,16 @@ class AP_Periph_FW {
uint16_t data_type_id,
uint8_t priority,
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,
CanardRxTransfer* transfer);
bool shouldAcceptTransfer(const CanardInstance* canard_instance,
Expand Down Expand Up @@ -465,7 +473,7 @@ class AP_Periph_FW {
#if HAL_PERIPH_CAN_MIRROR
void processMirror(void);
#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 node_status_send(void);
bool can_do_dna();
Expand Down
206 changes: 101 additions & 105 deletions Tools/AP_Periph/can.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,9 @@ extern AP_Periph_FW periph;
#endif
#endif

// timeout all frames at 1s
#define CAN_FRAME_TIMEOUT 1000000ULL

#define DEBUG_PKTS 0

#if HAL_PERIPH_CAN_MIRROR
Expand Down Expand Up @@ -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());

const int16_t resp_res = canardRequestOrRespond(canard_instance,
transfer->source_node_id,
UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE,
UAVCAN_PROTOCOL_GETNODEINFO_ID,
&transfer->transfer_id,
transfer->priority,
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);
}
canard_respond(canard_instance,
transfer,
UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE,
UAVCAN_PROTOCOL_GETNODEINFO_ID,
&buffer[0],
total_size);
}

/*
Expand Down Expand Up @@ -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] {};
uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer, !canfdout());

canardRequestOrRespond(canard_instance,
transfer->source_node_id,
UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE,
UAVCAN_PROTOCOL_PARAM_GETSET_ID,
&transfer->transfer_id,
transfer->priority,
CanardResponse,
&buffer[0],
total_size
#if CANARD_MULTI_IFACE
, IFACE_ALL
#endif
#if HAL_CANFD_SUPPORTED
,canfdout()
#endif
);

canard_respond(canard_instance,
transfer,
UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE,
UAVCAN_PROTOCOL_PARAM_GETSET_ID,
&buffer[0],
total_size);
}

/*
Expand Down Expand Up @@ -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] {};
uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer, !canfdout());

canardRequestOrRespond(canard_instance,
transfer->source_node_id,
UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE,
UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID,
&transfer->transfer_id,
transfer->priority,
CanardResponse,
&buffer[0],
total_size
#if CANARD_MULTI_IFACE
, IFACE_ALL
#endif
#if HAL_CANFD_SUPPORTED
,canfdout()
#endif
);
canard_respond(canard_instance,
transfer,
UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE,
UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID,
&buffer[0],
total_size);
}

void AP_Periph_FW::handle_begin_firmware_update(CanardInstance* canard_instance, CanardRxTransfer* transfer)
Expand Down Expand Up @@ -419,22 +388,12 @@ void AP_Periph_FW::handle_begin_firmware_update(CanardInstance* canard_instance,
reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK;

uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer, !canfdout());
canardRequestOrRespond(canard_instance,
transfer->source_node_id,
UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE,
UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID,
&transfer->transfer_id,
transfer->priority,
CanardResponse,
&buffer[0],
total_size
#if CANARD_MULTI_IFACE
,IFACE_ALL
#endif
#if HAL_CANFD_SUPPORTED
,canfdout()
#endif
);
canard_respond(canard_instance,
transfer,
UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE,
UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID,
&buffer[0],
total_size);
uint8_t count = 50;
while (count--) {
processTx();
Expand Down Expand Up @@ -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);
}

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);
}
Expand Down Expand Up @@ -1058,9 +1017,11 @@ bool AP_Periph_FW::canard_broadcast(uint64_t data_type_signature,
uint16_t data_type_id,
uint8_t priority,
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;
}

Expand All @@ -1069,21 +1030,69 @@ bool AP_Periph_FW::canard_broadcast(uint64_t data_type_signature,
return false;
}

const int16_t res = canardBroadcast(&dronecan.canard,
data_type_signature,
data_type_id,
tid_ptr,
priority,
payload,
payload_len
// create transfer object
CanardTxTransfer transfer_object = {
.transfer_type = CanardTransferTypeBroadcast,
.data_type_signature = data_type_signature,
.data_type_id = data_type_id,
.inout_transfer_id = tid_ptr,
.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
, IFACE_ALL // send over all ifaces
.iface_mask = iface_mask==0 ? uint8_t(IFACE_ALL) : iface_mask,
#endif
#if HAL_CANFD_SUPPORTED
, canfdout()
};
const int16_t res = canardBroadcastObj(&dronecan.canard, &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;
}

/*
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);
Expand Down Expand Up @@ -1151,15 +1160,17 @@ void AP_Periph_FW::processTx(void)
canardPopTxQueue(&dronecan.canard);
dronecan.tx_fail_count = 0;
} else {
// just exit and try again later. If we fail 8 times in a row
// then start discarding to prevent the pool filling up
// exit and try again later. If we fail 8 times in a row
// then cleanup any stale transfers to keep the queue from
// filling
if (dronecan.tx_fail_count < 8) {
dronecan.tx_fail_count++;
} else {
#if HAL_ENABLE_SENDING_STATS
protocol_stats.tx_errors++;
#endif
canardPopTxQueue(&dronecan.canard);
dronecan.tx_fail_count = 0;
cleanup_stale_transactions(now_us);
}
break;
}
Expand Down Expand Up @@ -1490,8 +1501,6 @@ bool AP_Periph_FW::can_do_dna()

const uint32_t now = AP_HAL::millis();

static uint8_t node_id_allocation_transfer_id = 0;

if (AP_Periph_FW::no_iface_finished_dna) {
printf("Waiting for dynamic node ID allocation %x... (pool %u)\n", IFACE_ALL, pool_peak_percent());
}
Expand Down Expand Up @@ -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);

// Broadcasting the request
const int16_t bcast_res = canardBroadcast(&dronecan.canard,
UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE,
UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
&node_id_allocation_transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&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);
}
canard_broadcast(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE,
UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&allocation_request[0],
(uint16_t) (uid_size + 1));

// Preparing for timeout; if response is received, this value will be updated from the callback.
dronecan.node_id_allocation_unique_id_offset = 0;
Expand Down
44 changes: 14 additions & 30 deletions Tools/AP_Periph/gps.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -163,10 +163,10 @@ void AP_Periph_FW::can_gps_update(void)
uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer, !canfdout());

canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE,
UAVCAN_EQUIPMENT_GNSS_FIX2_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
UAVCAN_EQUIPMENT_GNSS_FIX2_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
}

/*
Expand Down Expand Up @@ -258,34 +258,18 @@ void AP_Periph_FW::send_moving_baseline_msg()
uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {};
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)) {
uint8_t *tid_ptr = get_tid_ptr(MAKE_TRANSFER_DESCRIPTOR(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, 0, CANARD_BROADCAST_NODE_ID));
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);
iface_mask = 1U<<gps_mb_can_port;
}
#endif
canard_broadcast(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE,
ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size,
iface_mask);
gps.clear_RTCMV3();
#endif // GPS_MOVING_BASELINE
}
Expand Down

0 comments on commit be12f9e

Please sign in to comment.