From 3ed53c58e91c44e0dc4ae1645812b78c3005d631 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Sat, 19 Jun 2021 21:49:07 +0530 Subject: [PATCH] AP_Periph: provide separate instances of canard objects per iface --- Tools/AP_Periph/AP_Periph.cpp | 2 + Tools/AP_Periph/AP_Periph.h | 2 +- Tools/AP_Periph/can.cpp | 389 +++++++++++++++++++--------------- 3 files changed, 219 insertions(+), 174 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 3ef946e5b6..65e722c49c 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -86,7 +86,9 @@ void AP_Periph_FW::init() // always run with watchdog enabled. This should have already been // setup by the bootloader, but if not then enable now +#ifndef DISABLE_WATCHDOG stm32_watchdog_init(); +#endif stm32_watchdog_pat(); diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index d015a6c3aa..3808d4ba0d 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -130,7 +130,7 @@ public: #if HAL_NUM_CAN_IFACES >= 2 // This allows you to change the protocol and it continues to use the one at boot. // Without this, changing away from UAVCAN causes loss of comms and you can't - // change the rest of your params or veryofy it suceeded. + // change the rest of your params or verify it succeeded. AP_CANManager::Driver_Type can_protocol_cached[HAL_NUM_CAN_IFACES]; #endif diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index b9b2d8c9aa..65c9598889 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -79,13 +79,13 @@ extern AP_Periph_FW periph; #define HAL_CAN_POOL_SIZE 4000 #endif -static CanardInstance canard; -static uint32_t canard_memory_pool[HAL_CAN_POOL_SIZE/sizeof(uint32_t)]; +static CanardInstance canard[HAL_NUM_CAN_IFACES]; +static uint32_t canard_memory_pool[HAL_NUM_CAN_IFACES][HAL_CAN_POOL_SIZE/sizeof(uint32_t)]; #ifndef HAL_CAN_DEFAULT_NODE_ID #define HAL_CAN_DEFAULT_NODE_ID CANARD_BROADCAST_NODE_ID #endif static uint8_t PreferredNodeID = HAL_CAN_DEFAULT_NODE_ID; -static uint8_t transfer_id; +static uint8_t transfer_id[HAL_NUM_CAN_IFACES]; #ifndef CAN_APP_NODE_NAME #define CAN_APP_NODE_NAME "org.ardupilot.ap_periph" @@ -113,14 +113,27 @@ HALSITL::CANIface* AP_Periph_FW::can_iface_periph[HAL_NUM_CAN_IFACES]; * 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; ///< When the next node ID allocation request should be sent -static uint8_t node_id_allocation_unique_id_offset; ///< Depends on the stage of the next request +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 */ static uavcan_protocol_NodeStatus node_status; +/** + * Get interface id given canard object pointer + */ +static uint8_t get_canard_iface_id(CanardInstance* ins) +{ + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + if (ins == &canard[i]) { + return i; + } + } + // something is not right if we got here + return 0; +} /** * Returns a pseudo random integer in a given range @@ -350,6 +363,11 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr total_size); } +static int16_t canard_broadcast(uint64_t data_type_signature, + uint16_t data_type_id, + uint8_t priority, + const void* payload, + uint16_t payload_len); static void processTx(void); static void processRx(void); @@ -409,15 +427,16 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer) { + const uint8_t iface = get_canard_iface_id(ins); // Rule C - updating the randomized time interval - send_next_node_id_allocation_request_at_ms = + send_next_node_id_allocation_request_at_ms[iface] = 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); if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) { printf("Allocation request from another allocatee\n"); - node_id_allocation_unique_id_offset = 0; + node_id_allocation_unique_id_offset[iface] = 0; return; } @@ -437,14 +456,14 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr // Matching the received UID against the local one if (memcmp(received_unique_id, my_unique_id, received_unique_id_len) != 0) { printf("Mismatching allocation response\n"); - node_id_allocation_unique_id_offset = 0; + node_id_allocation_unique_id_offset[iface] = 0; return; // No match, return } 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. - node_id_allocation_unique_id_offset = received_unique_id_len; - send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; + node_id_allocation_unique_id_offset[iface] = received_unique_id_len; + send_next_node_id_allocation_request_at_ms[iface] -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; printf("Matching allocation response: %d\n", received_unique_id_len); } else { @@ -792,10 +811,8 @@ static void can_safety_button_update(void) uint8_t buffer[ARDUPILOT_INDICATION_BUTTON_MAX_SIZE] {}; uint16_t total_size = ardupilot_indication_Button_encode(&pkt, buffer); - canardBroadcast(&canard, - ARDUPILOT_INDICATION_BUTTON_SIGNATURE, + canard_broadcast(ARDUPILOT_INDICATION_BUTTON_SIGNATURE, ARDUPILOT_INDICATION_BUTTON_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); @@ -977,37 +994,72 @@ static bool shouldAcceptTransfer(const CanardInstance* ins, return false; } +static void cleanup_stale_transactions(uint64_t ×tamp_usec) +{ + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + canardCleanupStaleTransfers(&canard[i], timestamp_usec); + } +} + +static int16_t canard_broadcast(uint64_t data_type_signature, + uint16_t data_type_id, + uint8_t priority, + const void* payload, + uint16_t payload_len) +{ + int16_t cache = -1; + bool success = false; + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + if (canardGetLocalNodeID(&canard[i]) == CANARD_BROADCAST_NODE_ID) { + continue; + } + cache = canardBroadcast(&canard[i], + data_type_signature, + data_type_id, + &transfer_id[i], + priority, + payload, + payload_len); + if (cache == 0) { + // we had atleast one interface where transaction was successful + success = true; + } + } + return success?0:cache; +} + static void processTx(void) { - static uint8_t fail_count; - for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) { - AP_HAL::CANFrame txmsg {}; - txmsg.dlc = txf->data_len; - memcpy(txmsg.data, txf->data, 8); - txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF); - // push message with 1s timeout - bool sent_ok = false; - const uint64_t deadline = AP_HAL::native_micros64() + 1000000; - for (uint8_t i=0; i= 2 - if (periph.can_protocol_cached[i] != AP_CANManager::Driver_Type_UAVCAN) { - continue; - } -#endif - sent_ok |= (AP::periph().can_iface_periph[i]->send(txmsg, deadline, 0) > 0); + static uint8_t fail_count[HAL_NUM_CAN_IFACES]; + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + if (AP::periph().can_iface_periph[i] == NULL) { + continue; } - if (sent_ok) { - canardPopTxQueue(&canard); - 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 - if (fail_count < 8) { - fail_count++; +#if HAL_NUM_CAN_IFACES >= 2 + if (periph.can_protocol_cached[i] != AP_CANManager::Driver_Type_UAVCAN) { + continue; + } +#endif + for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard[i])) != NULL;) { + AP_HAL::CANFrame txmsg {}; + txmsg.dlc = txf->data_len; + memcpy(txmsg.data, txf->data, 8); + txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF); + // push message with 1s timeout + const uint64_t deadline = AP_HAL::native_micros64() + 1000000; + if (AP::periph().can_iface_periph[i]->send(txmsg, deadline, 0) > 0) { + canardPopTxQueue(&canard[i]); + fail_count[i] = 0; } else { - canardPopTxQueue(&canard); + // just exit and try again later. If we fail 8 times in a row + // then start discarding to prevent the pool filling up + if (fail_count[i] < 8) { + fail_count[i]++; + } else { + canardPopTxQueue(&canard[i]); + } + break; } - return; } } } @@ -1015,41 +1067,39 @@ static void processTx(void) static void processRx(void) { AP_HAL::CANFrame rxmsg; - while (true) { - bool got_pkt = false; - for (uint8_t i=0; i= 2 - if (periph.can_protocol_cached[i] != AP_CANManager::Driver_Type_UAVCAN) { - continue; - } + if (periph.can_protocol_cached[iface] != AP_CANManager::Driver_Type_UAVCAN) { + continue; + } #endif + while (true) { bool read_select = true; bool write_select = false; - AP::periph().can_iface_periph[i]->select(read_select, write_select, nullptr, 0); - if (!read_select) { - continue; + AP::periph().can_iface_periph[iface]->select(read_select, write_select, nullptr, 0); + if (!read_select) { // No data pending + break; } CanardCANFrame rx_frame {}; //palToggleLine(HAL_GPIO_PIN_LED); uint64_t timestamp; AP_HAL::CANIface::CanIOFlags flags; - AP::periph().can_iface_periph[i]->receive(rxmsg, timestamp, flags); + AP::periph().can_iface_periph[iface]->receive(rxmsg, timestamp, flags); memcpy(rx_frame.data, rxmsg.data, 8); rx_frame.data_len = rxmsg.dlc; rx_frame.id = rxmsg.id; - canardHandleRxFrame(&canard, &rx_frame, timestamp); - got_pkt = true; - } - if (!got_pkt) { - break; + canardHandleRxFrame(&canard[iface], &rx_frame, timestamp); } } } -static uint16_t pool_peak_percent(void) +static uint16_t pool_peak_percent(const uint8_t iface) { - const CanardPoolAllocatorStatistics stats = canardGetPoolAllocatorStatistics(&canard); + const CanardPoolAllocatorStatistics stats = canardGetPoolAllocatorStatistics(&canard[iface]); const uint16_t peak_percent = (uint16_t)(100U * stats.peak_usage_blocks / stats.capacity_blocks); return peak_percent; } @@ -1063,10 +1113,8 @@ static void node_status_send(void) uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); - const int16_t bc_res = canardBroadcast(&canard, - UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, + const int16_t bc_res = canard_broadcast(UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, UAVCAN_PROTOCOL_NODESTATUS_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, buffer, len); @@ -1086,7 +1134,7 @@ static void process1HzTasks(uint64_t timestamp_usec) /* * Purging transfers that are no longer transmitted. This will occasionally free up some memory. */ - canardCleanupStaleTransfers(&canard, timestamp_usec); + cleanup_stale_transactions(timestamp_usec); /* * Printing the memory usage statistics. @@ -1096,8 +1144,10 @@ 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 * record the worst case memory usage. */ - if (pool_peak_percent() > 70) { - printf("WARNING: ENLARGE MEMORY POOL\n"); + for (uint8_t iface = 0; iface < HAL_NUM_CAN_IFACES; iface++) { + if (pool_peak_percent(iface) > 70) { + printf("WARNING: ENLARGE MEMORY POOL on Iface %d\n", iface); + } } } @@ -1163,31 +1213,95 @@ static void process1HzTasks(uint64_t timestamp_usec) /* wait for dynamic allocation of node ID */ -static void can_wait_node_id(void) +static bool no_iface_finished_dna = true; +static bool can_do_dna(const uint8_t iface) { + if (canardGetLocalNodeID(&canard[iface]) != CANARD_BROADCAST_NODE_ID) { + no_iface_finished_dna = false; + return true; + } + uint8_t node_id_allocation_transfer_id = 0; + + if (no_iface_finished_dna) { + printf("Waiting for dynamic node ID allocation on IF%d... (pool %u)\n", iface, pool_peak_percent(iface)); + } + + const uint32_t now = AP_HAL::native_millis(); + + send_next_node_id_allocation_request_at_ms[iface] = + now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + + get_random_range(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); + + // Structure of the request is documented in the DSDL definition + // See http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation + uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1]; + allocation_request[0] = (uint8_t)(PreferredNodeID << 1U); + + if (node_id_allocation_unique_id_offset[iface] == 0) { + allocation_request[0] |= 1; // First part of unique ID + } + + uint8_t my_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH]; + readUniqueID(my_unique_id); + + 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]); + + if (uid_size > MaxLenOfUniqueIDInRequest) { + uid_size = MaxLenOfUniqueIDInRequest; + } + + memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset[iface]], uid_size); + + // Broadcasting the request + const int16_t bcast_res = canardBroadcast(&canard[iface], + 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 (bcast_res < 0) { + printf("Could not broadcast ID allocation req; error %d, IF%d\n", bcast_res, iface); + } + + // Preparing for timeout; if response is received, this value will be updated from the callback. + node_id_allocation_unique_id_offset[iface] = 0; + + if (no_iface_finished_dna) { + printf("Dynamic node ID allocation complete [IF%d][%d]\n", iface, canardGetLocalNodeID(&canard[iface])); + } + return false; +} + +/* + wait until atleast one iface has Node Allocation finished +*/ +static void can_wait_node_id() { + uint32_t now = AP_HAL::native_millis(); const uint32_t led_pattern = 0xAAAA; uint8_t led_idx = 0; uint32_t last_led_change = AP_HAL::native_millis(); const uint32_t led_change_period = 50; - while (canardGetLocalNodeID(&canard) == CANARD_BROADCAST_NODE_ID) - { - printf("Waiting for dynamic node ID allocation... (pool %u)\n", pool_peak_percent()); + while(true) { + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + if (can_do_dna(i)) { + return; + } + } + uint32_t wait_until_ms = UINT32_MAX; - stm32_watchdog_pat(); - uint32_t now = AP_HAL::native_millis(); - - send_next_node_id_allocation_request_at_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); - - while (((now=AP_HAL::native_millis()) < send_next_node_id_allocation_request_at_ms) && - (canardGetLocalNodeID(&canard) == CANARD_BROADCAST_NODE_ID)) - { + while (((now=AP_HAL::native_millis()) < wait_until_ms) && no_iface_finished_dna) { processTx(); processRx(); - canardCleanupStaleTransfers(&canard, AP_HAL::native_micros64()); + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + wait_until_ms = MIN(wait_until_ms, send_next_node_id_allocation_request_at_ms[i]); + } + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + canardCleanupStaleTransfers(&canard[i], AP_HAL::native_micros64()); + } stm32_watchdog_pat(); if (now - last_led_change > led_change_period) { @@ -1209,53 +1323,7 @@ static void can_wait_node_id(void) periph.check_for_serial_reboot_cmd(HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT); #endif } - - - if (canardGetLocalNodeID(&canard) != CANARD_BROADCAST_NODE_ID) - { - break; - } - - // Structure of the request is documented in the DSDL definition - // See http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation - uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1]; - allocation_request[0] = (uint8_t)(PreferredNodeID << 1U); - - if (node_id_allocation_unique_id_offset == 0) - { - allocation_request[0] |= 1; // First part of unique ID - } - - uint8_t my_unique_id[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH]; - readUniqueID(my_unique_id); - - 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); - if (uid_size > MaxLenOfUniqueIDInRequest) - { - uid_size = MaxLenOfUniqueIDInRequest; - } - - memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset], uid_size); - - // Broadcasting the request - const int16_t bcast_res = canardBroadcast(&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 (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. - node_id_allocation_unique_id_offset = 0; } - - printf("Dynamic node ID allocation complete [%d]\n", canardGetLocalNodeID(&canard)); } void AP_Periph_FW::can_start() @@ -1298,16 +1366,15 @@ void AP_Periph_FW::can_start() if (can_iface_periph[i] != nullptr) { 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]), + onTransferReceived, shouldAcceptTransfer, NULL); + + if (PreferredNodeID != CANARD_BROADCAST_NODE_ID) { + canardSetLocalNodeID(&canard[i], PreferredNodeID); + } } - canardInit(&canard, (uint8_t *)canard_memory_pool, sizeof(canard_memory_pool), - onTransferReceived, shouldAcceptTransfer, NULL); - - if (PreferredNodeID != CANARD_BROADCAST_NODE_ID) { - canardSetLocalNodeID(&canard, PreferredNodeID); - } - - // wait for dynamic node ID allocation + // wait for dynamic node ID allocation on atleast one iface can_wait_node_id(); } @@ -1356,10 +1423,8 @@ void AP_Periph_FW::pwm_hardpoint_update() uint8_t buffer[UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_hardpoint_Command_encode(&cmd, buffer); - canardBroadcast(&canard, - UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE, + canard_broadcast(UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE, UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); @@ -1390,10 +1455,8 @@ void AP_Periph_FW::hwesc_telem_update() uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer); - canardBroadcast(&canard, - UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, + canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, UAVCAN_EQUIPMENT_ESC_STATUS_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); @@ -1403,6 +1466,10 @@ void AP_Periph_FW::hwesc_telem_update() void AP_Periph_FW::can_update() { + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + can_do_dna(i); + } + static uint32_t last_1Hz_ms; uint32_t now = AP_HAL::native_millis(); if (now - last_1Hz_ms >= 1000) { @@ -1481,10 +1548,8 @@ void AP_Periph_FW::can_mag_update(void) uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_ahrs_MagneticFieldStrength_encode(&pkt, buffer); - canardBroadcast(&canard, - UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE, + canard_broadcast(UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE, UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); @@ -1547,10 +1612,8 @@ void AP_Periph_FW::can_battery_update(void) uint8_t buffer[UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE] {}; const uint16_t total_size = uavcan_equipment_power_BatteryInfo_encode(&pkt, buffer); - canardBroadcast(&canard, - UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE, + canard_broadcast(UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE, UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); @@ -1647,10 +1710,8 @@ void AP_Periph_FW::can_gps_update(void) uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_gnss_Fix_encode(&pkt, buffer); - canardBroadcast(&canard, - UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE, + canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE, UAVCAN_EQUIPMENT_GNSS_FIX_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); @@ -1740,10 +1801,8 @@ void AP_Periph_FW::can_gps_update(void) uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX2_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer); - canardBroadcast(&canard, - UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE, + canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE, UAVCAN_EQUIPMENT_GNSS_FIX2_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); @@ -1761,10 +1820,8 @@ void AP_Periph_FW::can_gps_update(void) uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_AUXILIARY_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_gnss_Auxiliary_encode(&aux, buffer); - canardBroadcast(&canard, - UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE, + canard_broadcast(UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE, UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); @@ -1790,10 +1847,8 @@ void AP_Periph_FW::can_gps_update(void) uint8_t buffer[ARDUPILOT_GNSS_STATUS_MAX_SIZE] {}; const uint16_t total_size = ardupilot_gnss_Status_encode(&status, buffer); - canardBroadcast(&canard, - ARDUPILOT_GNSS_STATUS_SIGNATURE, + canard_broadcast(ARDUPILOT_GNSS_STATUS_SIGNATURE, ARDUPILOT_GNSS_STATUS_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); @@ -1833,10 +1888,8 @@ void AP_Periph_FW::can_baro_update(void) uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_air_data_StaticPressure_encode(&pkt, buffer); - canardBroadcast(&canard, - UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_SIGNATURE, + canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); @@ -1853,10 +1906,8 @@ void AP_Periph_FW::can_baro_update(void) uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_air_data_StaticTemperature_encode(&pkt, buffer); - canardBroadcast(&canard, - UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_SIGNATURE, + canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); @@ -1918,10 +1969,8 @@ void AP_Periph_FW::can_airspeed_update(void) uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_air_data_RawAirData_encode(&pkt, buffer); - canardBroadcast(&canard, - UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE, + canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); @@ -1999,10 +2048,8 @@ void AP_Periph_FW::can_rangefinder_update(void) uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE] {}; uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer); - canardBroadcast(&canard, - UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, + canard_broadcast(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); @@ -2066,10 +2113,8 @@ void AP_Periph_FW::can_send_ADSB(struct __mavlink_adsb_vehicle_t &msg) uint8_t buffer[ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_MAX_SIZE] {}; uint16_t total_size = ardupilot_equipment_trafficmonitor_TrafficReport_encode(&pkt, buffer); - canardBroadcast(&canard, - ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SIGNATURE, + canard_broadcast(ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SIGNATURE, ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); @@ -2091,10 +2136,8 @@ void can_printf(const char *fmt, ...) uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer); - canardBroadcast(&canard, - UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, + canard_broadcast(UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, buffer, len);