Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_Periph: added CAN deadline handling #25780

Merged
merged 2 commits into from
Dec 19, 2023
Merged

AP_Periph: added CAN deadline handling #25780

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
97b6c85
waf: enable CAN deadlines in AP_Periph
tridge Dec 15, 2023
be12f9e
AP_Periph: use a 1s deadline for packets
tridge Dec 15, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 11 additions & 3 deletions Tools/AP_Periph/AP_Periph.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading