aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Periph: use range for loop instead of indexed for loops

Browse Source
This commit is contained in:
bugobliterator 2021-07-11 20:48:30 +05:30 committed by Andrew Tridgell
parent 67ed54bfca
commit 64c41e51ba
1 changed files with 88 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

161
Tools/AP_Periph/can.cpp
View File

@ -79,18 +79,35 @@ extern AP_Periph_FW periph;
#define HAL_CAN_POOL_SIZE 4000 #define HAL_CAN_POOL_SIZE 4000
#endif #endif
static CanardInstance canard[HAL_NUM_CAN_IFACES]; static struct instance_t {
static uint32_t canard_memory_pool[HAL_NUM_CAN_IFACES][HAL_CAN_POOL_SIZE/sizeof(uint32_t)]; uint8_t index;
#ifndef HAL_CAN_DEFAULT_NODE_ID CanardInstance canard;
#define HAL_CAN_DEFAULT_NODE_ID CANARD_BROADCAST_NODE_ID uint32_t canard_memory_pool[HAL_CAN_POOL_SIZE/sizeof(uint32_t)];
uint8_t transfer_id;
/*
* Variables used for dynamic node ID allocation.
* RTFM at http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation
*/
uint32_t send_next_node_id_allocation_request_at_ms; ///< When the next node ID allocation request should be sent
uint8_t node_id_allocation_unique_id_offset; ///< Depends on the stage of the next request
uint8_t tx_fail_count;
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
ChibiOS::CANIface* iface;
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
HALSITL::CANIface* iface;
#endif #endif
static uint8_t PreferredNodeID = HAL_CAN_DEFAULT_NODE_ID; } instances[HAL_NUM_CAN_IFACES];
static uint8_t transfer_id[HAL_NUM_CAN_IFACES];
#ifndef CAN_APP_NODE_NAME #ifndef CAN_APP_NODE_NAME
#define CAN_APP_NODE_NAME "org.ardupilot.ap_periph" #define CAN_APP_NODE_NAME "org.ardupilot.ap_periph"
#endif #endif
#ifndef HAL_CAN_DEFAULT_NODE_ID
#define HAL_CAN_DEFAULT_NODE_ID CANARD_BROADCAST_NODE_ID
#endif
uint8_t PreferredNodeID = HAL_CAN_DEFAULT_NODE_ID;
#ifndef AP_PERIPH_BATTERY_MODEL_NAME #ifndef AP_PERIPH_BATTERY_MODEL_NAME
#define AP_PERIPH_BATTERY_MODEL_NAME CAN_APP_NODE_NAME #define AP_PERIPH_BATTERY_MODEL_NAME CAN_APP_NODE_NAME
#endif #endif
@ -109,12 +126,6 @@ ChibiOS::CANIface* AP_Periph_FW::can_iface_periph[HAL_NUM_CAN_IFACES];
HALSITL::CANIface* AP_Periph_FW::can_iface_periph[HAL_NUM_CAN_IFACES]; HALSITL::CANIface* AP_Periph_FW::can_iface_periph[HAL_NUM_CAN_IFACES];
#endif #endif
/*
* Variables used for dynamic node ID allocation.
* RTFM at http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation
*/
static uint32_t send_next_node_id_allocation_request_at_ms[HAL_NUM_CAN_IFACES]; ///< When the next node ID allocation request should be sent
static uint8_t node_id_allocation_unique_id_offset[HAL_NUM_CAN_IFACES]; ///< Depends on the stage of the next request
/* /*
* Node status variables * Node status variables
@ -124,15 +135,15 @@ static uavcan_protocol_NodeStatus node_status;
/** /**
* Get interface id given canard object pointer * Get interface id given canard object pointer
*/ */
static uint8_t get_canard_iface_id(CanardInstance* ins) static instance_t* get_canard_iface_instance(CanardInstance* ins)
{ {
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { for (auto &can_ins : instances) {
if (ins == &canard[i]) { if (ins == &can_ins.canard) {
return i; return &can_ins;
} }
} }
// something is not right if we got here // something is not right if we got here
return 0; return nullptr;
} }
/** /**
@ -427,16 +438,19 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer*
static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer) static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer)
{ {
const uint8_t iface = get_canard_iface_id(ins); instance_t *can_ins = get_canard_iface_instance(ins);
if (can_ins == nullptr) {
return;
}
// Rule C - updating the randomized time interval // Rule C - updating the randomized time interval
send_next_node_id_allocation_request_at_ms[iface] = can_ins->send_next_node_id_allocation_request_at_ms =
AP_HAL::native_millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + AP_HAL::native_millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);
if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID)
{ {
printf("Allocation request from another allocatee\n"); printf("Allocation request from another allocatee\n");
node_id_allocation_unique_id_offset[iface] = 0; can_ins->node_id_allocation_unique_id_offset = 0;
return; return;
} }
@ -456,14 +470,14 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr
// Matching the received UID against the local one // Matching the received UID against the local one
if (memcmp(received_unique_id, my_unique_id, received_unique_id_len) != 0) { if (memcmp(received_unique_id, my_unique_id, received_unique_id_len) != 0) {
printf("Mismatching allocation response\n"); printf("Mismatching allocation response\n");
node_id_allocation_unique_id_offset[iface] = 0; can_ins->node_id_allocation_unique_id_offset = 0;
return; // No match, return return; // No match, return
} }
if (received_unique_id_len < UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH) { if (received_unique_id_len < UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH) {
// The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout. // The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout.
node_id_allocation_unique_id_offset[iface] = received_unique_id_len; can_ins->node_id_allocation_unique_id_offset = received_unique_id_len;
send_next_node_id_allocation_request_at_ms[iface] -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; can_ins->send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS;
printf("Matching allocation response: %d\n", received_unique_id_len); printf("Matching allocation response: %d\n", received_unique_id_len);
} else { } else {
@ -996,8 +1010,8 @@ static bool shouldAcceptTransfer(const CanardInstance* ins,
static void cleanup_stale_transactions(uint64_t &timestamp_usec) static void cleanup_stale_transactions(uint64_t &timestamp_usec)
{ {
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { for (auto &ins : instances) {
canardCleanupStaleTransfers(&canard[i], timestamp_usec); canardCleanupStaleTransfers(&ins.canard, timestamp_usec);
} }
} }
@ -1009,14 +1023,14 @@ static int16_t canard_broadcast(uint64_t data_type_signature,
{ {
int16_t cache = -1; int16_t cache = -1;
bool success = false; bool success = false;
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { for (auto &ins : instances) {
if (canardGetLocalNodeID(&canard[i]) == CANARD_BROADCAST_NODE_ID) { if (canardGetLocalNodeID(&ins.canard) == CANARD_BROADCAST_NODE_ID) {
continue; continue;
} }
cache = canardBroadcast(&canard[i], cache = canardBroadcast(&ins.canard,
data_type_signature, data_type_signature,
data_type_id, data_type_id,
&transfer_id[i], &ins.transfer_id,
priority, priority,
payload, payload,
payload_len); payload_len);
@ -1030,33 +1044,32 @@ static int16_t canard_broadcast(uint64_t data_type_signature,
static void processTx(void) static void processTx(void)
{ {
static uint8_t fail_count[HAL_NUM_CAN_IFACES]; for (auto &ins : instances) {
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { if (ins.iface == NULL) {
if (AP::periph().can_iface_periph[i] == NULL) {
continue; continue;
} }
#if HAL_NUM_CAN_IFACES >= 2 #if HAL_NUM_CAN_IFACES >= 2
if (periph.can_protocol_cached[i] != AP_CANManager::Driver_Type_UAVCAN) { if (periph.can_protocol_cached[ins.index] != AP_CANManager::Driver_Type_UAVCAN) {
continue; continue;
} }
#endif #endif
for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard[i])) != NULL;) { for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&ins.canard)) != NULL;) {
AP_HAL::CANFrame txmsg {}; AP_HAL::CANFrame txmsg {};
txmsg.dlc = txf->data_len; txmsg.dlc = txf->data_len;
memcpy(txmsg.data, txf->data, 8); memcpy(txmsg.data, txf->data, 8);
txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF); txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF);
// push message with 1s timeout // push message with 1s timeout
const uint64_t deadline = AP_HAL::native_micros64() + 1000000; const uint64_t deadline = AP_HAL::native_micros64() + 1000000;
if (AP::periph().can_iface_periph[i]->send(txmsg, deadline, 0) > 0) { if (ins.iface->send(txmsg, deadline, 0) > 0) {
canardPopTxQueue(&canard[i]); canardPopTxQueue(&ins.canard);
fail_count[i] = 0; ins.tx_fail_count = 0;
} else { } else {
// just exit and try again later. If we fail 8 times in a row // just exit and try again later. If we fail 8 times in a row
// then start discarding to prevent the pool filling up // then start discarding to prevent the pool filling up
if (fail_count[i] < 8) { if (ins.tx_fail_count < 8) {
fail_count[i]++; ins.tx_fail_count++;
} else { } else {
canardPopTxQueue(&canard[i]); canardPopTxQueue(&ins.canard);
} }
break; break;
} }
@ -1067,19 +1080,19 @@ static void processTx(void)
static void processRx(void) static void processRx(void)
{ {
AP_HAL::CANFrame rxmsg; AP_HAL::CANFrame rxmsg;
for (uint8_t iface=0; iface<HAL_NUM_CAN_IFACES; iface++) { for (auto &ins : instances) {
if (AP::periph().can_iface_periph[iface] == NULL) { if (ins.iface == NULL) {
continue; continue;
} }
#if HAL_NUM_CAN_IFACES >= 2 #if HAL_NUM_CAN_IFACES >= 2
if (periph.can_protocol_cached[iface] != AP_CANManager::Driver_Type_UAVCAN) { if (periph.can_protocol_cached[ins.index] != AP_CANManager::Driver_Type_UAVCAN) {
continue; continue;
} }
#endif #endif
while (true) { while (true) {
bool read_select = true; bool read_select = true;
bool write_select = false; bool write_select = false;
AP::periph().can_iface_periph[iface]->select(read_select, write_select, nullptr, 0); ins.iface->select(read_select, write_select, nullptr, 0);
if (!read_select) { // No data pending if (!read_select) { // No data pending
break; break;
} }
@ -1088,18 +1101,18 @@ static void processRx(void)
//palToggleLine(HAL_GPIO_PIN_LED); //palToggleLine(HAL_GPIO_PIN_LED);
uint64_t timestamp; uint64_t timestamp;
AP_HAL::CANIface::CanIOFlags flags; AP_HAL::CANIface::CanIOFlags flags;
AP::periph().can_iface_periph[iface]->receive(rxmsg, timestamp, flags); ins.iface->receive(rxmsg, timestamp, flags);
memcpy(rx_frame.data, rxmsg.data, 8); memcpy(rx_frame.data, rxmsg.data, 8);
rx_frame.data_len = rxmsg.dlc; rx_frame.data_len = rxmsg.dlc;
rx_frame.id = rxmsg.id; rx_frame.id = rxmsg.id;
canardHandleRxFrame(&canard[iface], &rx_frame, timestamp); canardHandleRxFrame(&ins.canard, &rx_frame, timestamp);
} }
} }
} }
static uint16_t pool_peak_percent(const uint8_t iface) static uint16_t pool_peak_percent(instance_t &ins)
{ {
const CanardPoolAllocatorStatistics stats = canardGetPoolAllocatorStatistics(&canard[iface]); const CanardPoolAllocatorStatistics stats = canardGetPoolAllocatorStatistics(&ins.canard);
const uint16_t peak_percent = (uint16_t)(100U * stats.peak_usage_blocks / stats.capacity_blocks); const uint16_t peak_percent = (uint16_t)(100U * stats.peak_usage_blocks / stats.capacity_blocks);
return peak_percent; return peak_percent;
} }
@ -1144,9 +1157,9 @@ static void process1HzTasks(uint64_t timestamp_usec)
* The recommended way to establish the minimal size of the memory pool is to stress-test the application and * The recommended way to establish the minimal size of the memory pool is to stress-test the application and
* record the worst case memory usage. * record the worst case memory usage.
*/ */
for (uint8_t iface = 0; iface < HAL_NUM_CAN_IFACES; iface++) { for (auto &ins : instances) {
if (pool_peak_percent(iface) > 70) { if (pool_peak_percent(ins) > 70) {
printf("WARNING: ENLARGE MEMORY POOL on Iface %d\n", iface); printf("WARNING: ENLARGE MEMORY POOL on Iface %d\n", ins.index);
} }
} }
} }
@ -1214,9 +1227,9 @@ static void process1HzTasks(uint64_t timestamp_usec)
wait for dynamic allocation of node ID wait for dynamic allocation of node ID
*/ */
static bool no_iface_finished_dna = true; static bool no_iface_finished_dna = true;
static bool can_do_dna(const uint8_t iface) static bool can_do_dna(instance_t &ins)
{ {
if (canardGetLocalNodeID(&canard[iface]) != CANARD_BROADCAST_NODE_ID) { if (canardGetLocalNodeID(&ins.canard) != CANARD_BROADCAST_NODE_ID) {
no_iface_finished_dna = false; no_iface_finished_dna = false;
return true; return true;
} }
@ -1224,12 +1237,12 @@ static bool can_do_dna(const uint8_t iface)
uint8_t node_id_allocation_transfer_id = 0; uint8_t node_id_allocation_transfer_id = 0;
if (no_iface_finished_dna) { if (no_iface_finished_dna) {
printf("Waiting for dynamic node ID allocation on IF%d... (pool %u)\n", iface, pool_peak_percent(iface)); printf("Waiting for dynamic node ID allocation on IF%d... (pool %u)\n", ins.index, pool_peak_percent(ins));
} }
const uint32_t now = AP_HAL::native_millis(); const uint32_t now = AP_HAL::native_millis();
send_next_node_id_allocation_request_at_ms[iface] = ins.send_next_node_id_allocation_request_at_ms =
now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);
@ -1238,7 +1251,7 @@ static bool can_do_dna(const uint8_t iface)
uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1]; uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1];
allocation_request[0] = (uint8_t)(PreferredNodeID << 1U); allocation_request[0] = (uint8_t)(PreferredNodeID << 1U);
if (node_id_allocation_unique_id_offset[iface] == 0) { if (ins.node_id_allocation_unique_id_offset == 0) {
allocation_request[0] |= 1; // First part of unique ID allocation_request[0] |= 1; // First part of unique ID
} }
@ -1246,16 +1259,16 @@ static bool can_do_dna(const uint8_t iface)
readUniqueID(my_unique_id); readUniqueID(my_unique_id);
static const uint8_t MaxLenOfUniqueIDInRequest = 6; static const uint8_t MaxLenOfUniqueIDInRequest = 6;
uint8_t uid_size = (uint8_t)(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH - node_id_allocation_unique_id_offset[iface]); uint8_t uid_size = (uint8_t)(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH - ins.node_id_allocation_unique_id_offset);
if (uid_size > MaxLenOfUniqueIDInRequest) { if (uid_size > MaxLenOfUniqueIDInRequest) {
uid_size = MaxLenOfUniqueIDInRequest; uid_size = MaxLenOfUniqueIDInRequest;
} }
memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset[iface]], uid_size); memmove(&allocation_request[1], &my_unique_id[ins.node_id_allocation_unique_id_offset], uid_size);
// Broadcasting the request // Broadcasting the request
const int16_t bcast_res = canardBroadcast(&canard[iface], const int16_t bcast_res = canardBroadcast(&ins.canard,
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,
&node_id_allocation_transfer_id, &node_id_allocation_transfer_id,
@ -1263,14 +1276,14 @@ static bool can_do_dna(const uint8_t iface)
&allocation_request[0], &allocation_request[0],
(uint16_t) (uid_size + 1)); (uint16_t) (uid_size + 1));
if (bcast_res < 0) { if (bcast_res < 0) {
printf("Could not broadcast ID allocation req; error %d, IF%d\n", bcast_res, iface); printf("Could not broadcast ID allocation req; error %d, IF%d\n", bcast_res, ins.index);
} }
// 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.
node_id_allocation_unique_id_offset[iface] = 0; ins.node_id_allocation_unique_id_offset = 0;
if (no_iface_finished_dna) { if (no_iface_finished_dna) {
printf("Dynamic node ID allocation complete [IF%d][%d]\n", iface, canardGetLocalNodeID(&canard[iface])); printf("Dynamic node ID allocation complete [IF%d][%d]\n", ins.index, canardGetLocalNodeID(&ins.canard));
} }
return false; return false;
} }
@ -1286,8 +1299,8 @@ static void can_wait_node_id() {
const uint32_t led_change_period = 50; const uint32_t led_change_period = 50;
while(true) { while(true) {
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { for (auto &ins : instances) {
if (can_do_dna(i)) { if (can_do_dna(ins)) {
return; return;
} }
} }
@ -1296,11 +1309,11 @@ static void can_wait_node_id() {
while (((now=AP_HAL::native_millis()) < wait_until_ms) && no_iface_finished_dna) { while (((now=AP_HAL::native_millis()) < wait_until_ms) && no_iface_finished_dna) {
processTx(); processTx();
processRx(); processRx();
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { for (auto &ins : instances) {
wait_until_ms = MIN(wait_until_ms, send_next_node_id_allocation_request_at_ms[i]); wait_until_ms = MIN(wait_until_ms, ins.send_next_node_id_allocation_request_at_ms);
} }
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { for (auto &ins : instances) {
canardCleanupStaleTransfers(&canard[i], AP_HAL::native_micros64()); canardCleanupStaleTransfers(&ins.canard, AP_HAL::native_micros64());
} }
stm32_watchdog_pat(); stm32_watchdog_pat();
@ -1359,6 +1372,8 @@ void AP_Periph_FW::can_start()
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
can_iface_periph[i] = new HALSITL::CANIface(); can_iface_periph[i] = new HALSITL::CANIface();
#endif #endif
instances[i].iface = can_iface_periph[i];
instances[i].index = i;
#if HAL_NUM_CAN_IFACES >= 2 #if HAL_NUM_CAN_IFACES >= 2
can_protocol_cached[i] = g.can_protocol[i]; can_protocol_cached[i] = g.can_protocol[i];
CANSensor::set_periph(i, can_protocol_cached[i], can_iface_periph[i]); CANSensor::set_periph(i, can_protocol_cached[i], can_iface_periph[i]);
@ -1366,11 +1381,11 @@ void AP_Periph_FW::can_start()
if (can_iface_periph[i] != nullptr) { if (can_iface_periph[i] != nullptr) {
can_iface_periph[i]->init(g.can_baudrate[i], AP_HAL::CANIface::NormalMode); can_iface_periph[i]->init(g.can_baudrate[i], AP_HAL::CANIface::NormalMode);
} }
canardInit(&canard[i], (uint8_t *)canard_memory_pool[i], sizeof(canard_memory_pool[i]), canardInit(&instances[i].canard, (uint8_t *)instances[i].canard_memory_pool, sizeof(instances[i].canard_memory_pool),
onTransferReceived, shouldAcceptTransfer, NULL); onTransferReceived, shouldAcceptTransfer, NULL);
if (PreferredNodeID != CANARD_BROADCAST_NODE_ID) { if (PreferredNodeID != CANARD_BROADCAST_NODE_ID) {
canardSetLocalNodeID(&canard[i], PreferredNodeID); canardSetLocalNodeID(&instances[i].canard, PreferredNodeID);
} }
} }
@ -1466,8 +1481,8 @@ void AP_Periph_FW::hwesc_telem_update()
void AP_Periph_FW::can_update() void AP_Periph_FW::can_update()
{ {
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { for (auto &ins : instances) {
can_do_dna(i); can_do_dna(ins);
} }
static uint32_t last_1Hz_ms; static uint32_t last_1Hz_ms;