#include "uavcan.h" #include #include #include "main.h" #include "math.h" #include "can.h" #include "stm32f1xx_hal_tim.h" #define MEMPOOL_OBJECTS 16 #define ALLOCATION_TIMEOUT_MS 60000 #define DEFAULT_NODE_ID 60 static void handle_begin_firmware_update(CanardInstance *ins, CanardRxTransfer *transfer); static uint8_t is_node_restart_requested(void); static uint8_t is_node_restart_requested(void); static const uint8_t RESTART_NODE_MAGIC_NUMBER_ARRAY[UAVCAN_RESTART_NODE_REQUEST_MAX_SIZE] = {0x1E, 0x1B, 0x55, 0xCE, 0xAC}; static CanardInstance g_canard; //The library instance static uint8_t g_canard_memory_pool[2048]; static uint32_t spin_time = 0; uint8_t feedback_status = 0; static uint8_t node_unique_id[UNIQUE_ID_LENGTH_BYTES]; //static const uint8_t gimbal_id = 0xC5; //Arena for memory allocation, used by the library //static uint32_t g_uptime = 0; //static uint8_t node_health = UAVCAN_NODE_HEALTH_OK; //static uint8_t node_mode = UAVCAN_NODE_MODE_INITIALIZATION; static uint64_t send_next_node_id_allocation_request_at; ///< When the next node ID allocation request should be sent static uint8_t node_id_allocation_unique_id_offset; static uint8_t transfer_id = 0; uint32_t node_restart_at; //TODO check where used uint32_t last_invocation; uint32_t allocation_init_tick; //static uint32_t last_state_time = 0; //extern QueueHandle_t uavcan_rev_queue; typedef struct { // FieldTypes uint8_t id; // bit len 8 float pitch; // float16 Saturate float roll; // float16 Saturate float yaw; // float16 Saturate } zrzk_equipment_uav_Euler; static struct { uint64_t request_timer_begin_us; uint64_t request_delay_us; uint32_t unique_id_offset; } allocation_state; #ifndef CANARD_INTERNAL_SATURATE #define CANARD_INTERNAL_SATURATE(x, max) (((x) > max) ? max : ((-(x) > max) ? (-max) : (x))); #endif #ifndef CANARD_INTERNAL_SATURATE_UNSIGNED #define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) (((x) >= max) ? max : (x)); #endif #if defined(__GNUC__) #define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) #else #define CANARD_MAYBE_UNUSED(x) x #endif extern TIM_HandleTypeDef htim2; static void allocation_timer_expired(void); static void handle_allocation_data_broadcast(CanardInstance *ins, CanardRxTransfer *transfer); static void allocation_start_request_timer(void); static void allocation_start_followup_timer(void); static bool allocation_running(void); static float uavcan_get_radom_float(void); uint64_t micros(void); static void allocation_init(void); static void allocation_timer_expired(void); bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id) { // (void)source_node_id; if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) { // If we're in the process of allocation of dynamic node ID, accept only relevant transfers. if ((transfer_type == CanardTransferTypeBroadcast) && (data_type_id == UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID)) { *out_data_type_signature = UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE; return true; } else { return false; } } else { switch (data_type_id) { case UAVCAN_GET_NODE_INFO_DATA_TYPE_ID: *out_data_type_signature = UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE; return true; case UAVCAN_RESTART_NODE_DATA_TYPE_ID: *out_data_type_signature = UAVCAN_RESTART_NODE_DATA_TYPE_SIGNATURE; return true; case UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID: *out_data_type_signature = UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_SIGNATURE; return true; case UAVCAN_FILE_READ_DATA_TYPE_ID: *out_data_type_signature = UAVCAN_FILE_READ_DATA_TYPE_SIGNATURE; return true; case UAVCAN_PARAM_GETSET_DATA_TYPE_ID: *out_data_type_signature = UAVCAN_PARAM_GETSET_DATA_TYPE_SIGNATURE; return true; default: return false; } } } void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { if (transfer->transfer_type == CanardTransferTypeBroadcast) { action_transfer_received_broadcast(ins, transfer); } else if (transfer->transfer_type == CanardTransferTypeResponse) { action_transfer_received_response(ins, transfer); } else { action_transfer_received_request(ins, transfer); } } void action_transfer_received_request(CanardInstance *ins, CanardRxTransfer *transfer) { switch (transfer->data_type_id) { case UAVCAN_GET_NODE_INFO_DATA_TYPE_ID: handle_canard_get_node_info(transfer); break; case UAVCAN_RESTART_NODE_DATA_TYPE_ID: handle_restart_node(transfer); break; case UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID: handle_begin_firmware_update(ins, transfer); break; case UAVCAN_PARAM_GETSET_DATA_TYPE_ID: // handle_param_getset(transfer); break; default: break; } } void action_transfer_received_broadcast(CanardInstance *ins, CanardRxTransfer *transfer) { switch (transfer->data_type_id) { case UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID: if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) { handle_allocation_data_broadcast(&g_canard, transfer); } break; default: break; } } void action_transfer_received_response(CanardInstance *ins, CanardRxTransfer *transfer) { switch (transfer->data_type_id) { default: break; } } void receive_canard(void) { CanardCANFrame rx_frame; uint32_t time_stame = HAL_GetTick(); while (canardSTM32Receive(&rx_frame) > 0) { canardHandleRxFrame(&g_canard, &rx_frame, HAL_GetTick()); if ((HAL_GetTick() - time_stame) > 100) { return; } } } void handle_uavcan_rx_frame(const uavcan_rev_t *msg) { canardHandleRxFrame(&g_canard, &msg->canard_CAN_frame, msg->tick_ms * 1000); } static void handle_canard_get_node_info(CanardRxTransfer *transfer) { uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE]; memset(buffer, 0, UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE); uint16_t len = 0; len = make_node_info_message(buffer); int result = canardRequestOrRespond(&g_canard, transfer->source_node_id, UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE, UAVCAN_GET_NODE_INFO_DATA_TYPE_ID, &transfer->transfer_id, transfer->priority, CanardResponse, &buffer[0], (uint16_t)len); } void uavcan_init(void) { CanardSTM32CANTimings timings; int result = canardSTM32ComputeCANTimings(HAL_RCC_GetPCLK1Freq(), 1000000, &timings); if (result) { __ASM volatile("BKPT #01"); } result = canardSTM32Init(&timings, CanardSTM32IfaceModeNormal); if (result) { __ASM volatile("BKPT #01"); } canardInit(&g_canard, // Uninitialized library instance g_canard_memory_pool, // Raw memory chunk used for dynamic allocation sizeof(g_canard_memory_pool), // Size of the above, in bytes onTransferReceived, // Callback, see CanardOnTransferReception shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer NULL); canardSetLocalNodeID(&g_canard, uavcan_local_node); } void uavcan_init_with_cubemx(void) { uavcan_init_fifo(uavcan_local_node); canardInit(&g_canard, // Uninitialized library instance g_canard_memory_pool, // Raw memory chunk used for dynamic allocation sizeof(g_canard_memory_pool), // Size of the above, in bytes onTransferReceived, // Callback, see CanardOnTransferReception shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer NULL); canardSetLocalNodeID(&g_canard, uavcan_local_node); can_init_irq(); } void send_canard(void) { osStatus_t status; if (uavcan_send_mutexHandle != NULL) { status = osMutexAcquire(uavcan_send_mutexHandle, 20U); } for (const CanardCANFrame *txf = NULL; (txf = canardPeekTxQueue(&g_canard)) != NULL;) { const int16_t tx_res = canardSTM32Transmit(txf); if (tx_res < 0) // Failure - drop the frame and report { break; } else if (tx_res > 0) // Success - just drop the frame { canardPopTxQueue(&g_canard); } else // Timeout - just exit and try again later { break; } } if (status == osOK && uavcan_send_mutexHandle != NULL) { status = osMutexRelease(uavcan_send_mutexHandle); } } void publish_node_status(void) { if (HAL_GetTick() < spin_time + CANARD_SPIN_PERIOD) return; // rate limiting spin_time = HAL_GetTick(); uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE]; static uint8_t transfer_id = 0; // This variable MUST BE STATIC; refer to the libcanard documentation for the background make_node_status_message(buffer); canardBroadcast(&g_canard, UAVCAN_NODE_STATUS_DATA_TYPE_SIGNATURE, UAVCAN_NODE_STATUS_DATA_TYPE_ID, &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, buffer, UAVCAN_NODE_STATUS_MESSAGE_SIZE); } void make_node_status_message(uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE]) { uint8_t node_health = UAVCAN_NODE_HEALTH_OK; uint8_t node_mode = UAVCAN_NODE_MODE_OPERATIONAL; memset(buffer, 0, UAVCAN_NODE_STATUS_MESSAGE_SIZE); uint32_t uptime_sec = (HAL_GetTick() / 1000); canardEncodeScalar(buffer, 0, 32, &uptime_sec); canardEncodeScalar(buffer, 32, 2, &node_health); canardEncodeScalar(buffer, 34, 3, &node_mode); } uint16_t make_node_info_message(uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE]) { memset(buffer, 0, UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE); make_node_status_message(buffer); buffer[7] = APP_VERSION_MAJOR; buffer[8] = APP_VERSION_MINOR; buffer[9] = 1; // Optional field flags, VCS commit is set uint32_t githash = GIT_HASH; canardEncodeScalar(buffer, 80, 32, &githash); buffer[22] = 0; //get_hw_version_minor(); buffer[23] = 0; //get_hw_version_patch(); read_unique_id(&buffer[24]); const size_t name_len = strlen(APP_NODE_NAME); memcpy(&buffer[41], APP_NODE_NAME, name_len); return 41 + name_len; } void canard_get_node_info_handle(CanardRxTransfer *transfer) { uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE]; memset(buffer, 0, UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE); uint16_t len = 0; len = make_node_info_message(buffer); int result = canardRequestOrRespond(&g_canard, transfer->source_node_id, UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE, UAVCAN_GET_NODE_INFO_DATA_TYPE_ID, &transfer->transfer_id, // transfer->priority, CANARD_TRANSFER_PRIORITY_HIGH, CanardResponse, &buffer[0], (uint16_t)len); } /** * Returns a pseudo random float in the range [0, 1]. */ static float get_random_float(void) { static bool initialized = false; if (!initialized) { initialized = true; // desig_get_unique_id((uint32_t*)&node_unique_id[0]); // const uint32_t *unique_32 = (uint32_t *)&node_unique_id[0]; // srand(HAL_GetTick() ^ *unique_32); srand(HAL_GetTick()); } return (float)rand() / (float)RAND_MAX; } void publish_log_message(uavcan_log_level_t log_level, const char *source, const char *text) { uint8_t buffer[UAVCAN_DEBUG_LOG_MESSAGE_MESSAGE_SIZE] = {0}; uint8_t source_len = strlen(source) & 31; uint8_t msg_len = strlen(text); static uint8_t transfer_id; if (msg_len > 90) { msg_len = 90; } /* Use manual mutex locking here to lock the global buffer as well */ buffer[0] = (log_level << 5) | source_len; memcpy(&buffer[1], source, source_len); memcpy(&buffer[1 + source_len], text, msg_len); canardBroadcast( &g_canard, UAVCAN_DEBUG_LOG_MESSAGE_DATA_TYPE_SIGNATURE, UAVCAN_DEBUG_LOG_MESSAGE_DATA_TYPE_ID, &transfer_id, CANARD_TRANSFER_PRIORITY_LOWEST, buffer, 1 + source_len + msg_len); } void send_uavcan_data(uavcan_send_data_t *send_data) { switch (send_data->id) { case UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID: /* code */ break; case UAVCAN_NODE_STATUS_DATA_TYPE_ID: publish_node_status(); break; case ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_ID: publish_proximity(&send_data->content.proximity); break; default: break; } } void publish_proximity(uavcan_proximity_t *msg) { uint8_t buffer[ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_MAX_SIZE] = {0}; uint32_t len = zrzk_equipment_range_sensor_Proximity_encode(msg, &buffer); if (len > 0) { canardBroadcast( &g_canard, ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_SIGNATURE, ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_ID, &transfer_id, CANARD_TRANSFER_PRIORITY_HIGH, buffer, len); } } void send_uavcan_proximity(uavcan_proximity_t *data) { uavcan_send_data_t msg = {0}; if (uavcan_send_queueHandle != NULL) { msg.id = ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_ID; memcpy(&msg.content.proximity, data, sizeof(uavcan_proximity_t)); osMessageQueuePut(uavcan_send_queueHandle, &msg, NULL, 10U); } } void request_node_restart(void) { memset(&bootloader_interface, 0, sizeof(bootloader_interface)); bootloader_interface.node_id = get_node_id(); //config_get_uint(CONFIG_NODE_ID); //TODO use param bootloader_interface.magic = BOOTLOADER_INTERFACE_VALID_MAGIC; if (node_restart_at == 0) { node_restart_at = HAL_GetTick() + 1000; //chVTGetSystemTimeX() + TIME_S2I(1); } } void check_need_to_jump_boot(void) { if ((last_invocation + 1000) < HAL_GetTick()) { // process1HzTasks(getMonotonicTimestamp()); last_invocation = HAL_GetTick(); /* Intentionally exclude processing time */ if (node_restart_at && (HAL_GetTick() > node_restart_at)) { NVIC_SystemReset(); } } } static uint8_t is_node_restart_requested(void) { return (node_restart_at != 0); } static void handle_begin_firmware_update(CanardInstance *ins, CanardRxTransfer *transfer) { uint8_t response = 0; uint8_t filename_length = transfer->payload_len - 1; /* Tail array optimisation for this data type */ uint8_t source_id = transfer->payload_head[0]; if (is_node_restart_requested()) { response = 1; } if (response == 0) { bootloader_interface.request_from_node_id = source_id; bootloader_interface.request_file_name_length = filename_length; for (uint8_t i = 0; i < filename_length; ++i) { canardDecodeScalar(transfer, 8 + 8 * i, 8, 0, &bootloader_interface.request_file_name[i]); } request_node_restart(); } //canardLock(canardReleaseRxTransferPayload(ins, transfer)); const int resp_res = canardRequestOrRespond(ins, transfer->source_node_id, UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_SIGNATURE, UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID, &transfer->transfer_id, transfer->priority, CanardResponse, &response, 1); } static void read_unique_id(uint8_t *out_uid) { memcpy(out_uid, (void *)UID_BASE, UNIQUE_STM32_DEVICE_ID_LENGTH); } uint8_t get_node_id(void) { return canardGetLocalNodeID(&g_canard); } void uavcan_init_fifo(uint8_t node_id) { uint32_t id = 0; uint32_t mask = 0; uint8_t bank_index = 1; uavcan_get_uavcan_id_mask_by_msg_id(UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID, &id, &mask); can_filter_init_with_mask(bank_index, id, mask, 1); bank_index++; uavcan_get_uavcan_id_mask_by_msg_id(ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_ID, &id, &mask); can_filter_init_with_mask(bank_index, id, mask, 1); bank_index++; uavcan_get_uavcan_id_mask_by_msg_id(UAVCAN_RESTART_NODE_DATA_TYPE_ID, &id, &mask); can_filter_init_with_mask(bank_index, id, mask, 1); bank_index++; uavcan_get_uavcan_id_mask_by_node_id(node_id, &id, &mask); can_filter_init_with_mask(bank_index, id, mask, 1); bank_index++; } void uavcan_cleanup_stale_transfers(uint64_t time_us) { canardCleanupStaleTransfers(&g_canard, time_us); } void handle_restart_node(CanardRxTransfer *transfer) { uint8_t response = 0; if (memcmp(transfer->payload_head, RESTART_NODE_MAGIC_NUMBER_ARRAY, UAVCAN_RESTART_NODE_REQUEST_MAX_SIZE) == 0) { response = 1; /* Asynchronous restart so we can send response package first. */ request_node_restart(); } const int resp_res = canardRequestOrRespond(&g_canard, transfer->source_node_id, UAVCAN_RESTART_NODE_DATA_TYPE_SIGNATURE, UAVCAN_RESTART_NODE_DATA_TYPE_ID, &transfer->transfer_id, transfer->priority, CanardResponse, &response, 1); } static void allocation_init(void) { if (!allocation_running()) { return; } // Start request timer allocation_start_request_timer(); } void allocation_update(void) { if (!allocation_running()) { return; } // Check allocation timer if (micros() - allocation_state.request_timer_begin_us > allocation_state.request_delay_us) { //printf("us:%llu ,ms:%u\r\n", micros(), HAL_GetTick()); // printf("del:%llu,requst_us:%llu,begin_us:%llu\r\n", // micros() - allocation_state.request_timer_begin_us, // allocation_state.request_delay_us, // allocation_state.request_timer_begin_us); allocation_timer_expired(); // boot_application_at = HAL_GetTick() + 2000; } if ((allocation_init_tick + ALLOCATION_TIMEOUT_MS) < HAL_GetTick()) { if (bootloader_interface.magic == BOOTLOADER_INTERFACE_VALID_MAGIC && bootloader_interface.node_id) { canardSetLocalNodeID(&g_canard, bootloader_interface.node_id); init_uavcan_fifo(get_node_id()); } else { canardSetLocalNodeID(&g_canard, DEFAULT_NODE_ID); init_uavcan_fifo(get_node_id()); } } } static void allocation_timer_expired(void) { if (!allocation_running()) { return; } // Start allocation request timer allocation_start_request_timer(); // Send allocation message uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1]; uint8_t uid_size = MIN(UNIQUE_ID_LENGTH_BYTES - allocation_state.unique_id_offset, UAVCAN_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UID_IN_REQUEST); allocation_request[0] = (allocation_state.unique_id_offset == 0) ? 1 : 0; //printf("allocation_request:%d \r\n",allocation_request[0]); memcpy(&allocation_request[1], &node_unique_id[allocation_state.unique_id_offset], uid_size); static uint8_t transfer_id; canardBroadcast(&g_canard, UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE, UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID, &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, allocation_request, uid_size + 1); allocation_state.unique_id_offset = 0; } static void handle_allocation_data_broadcast(CanardInstance *ins, CanardRxTransfer *transfer) { if (!allocation_running()) { return; } // Always start the allocation request timer and reset the UID offset allocation_start_request_timer(); allocation_state.unique_id_offset = 0; if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) { // If source node ID is anonymous, return return; } uint8_t received_unique_id[UNIQUE_ID_LENGTH_BYTES]; uint8_t received_unique_id_len = transfer->payload_len - 1; uint8_t i; for (i = 0; i < received_unique_id_len; i++) { canardDecodeScalar(transfer, i * 8 + UAVCAN_NODE_ID_ALLOCATION_UID_BIT_OFFSET, 8, false, &received_unique_id[i]); } if (memcmp(node_unique_id, received_unique_id, received_unique_id_len) != 0) { // If unique ID does not match, return return; } if (received_unique_id_len < UNIQUE_ID_LENGTH_BYTES) { // Unique ID partially matches - set the UID offset and start the followup timer allocation_state.unique_id_offset = received_unique_id_len; allocation_start_followup_timer(); } else { // Complete match received uint8_t allocated_node_id = 0; canardDecodeScalar(transfer, 0, 7, false, &allocated_node_id); if (allocated_node_id != 0) { canardSetLocalNodeID(ins, allocated_node_id); uavcan_init_fifo(canardGetLocalNodeID(&g_canard)); } } } static void allocation_start_request_timer(void) { if (!allocation_running()) { return; } allocation_state.request_timer_begin_us = micros(); allocation_state.request_delay_us = UAVCAN_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_US + (uint32_t)(uavcan_get_radom_float() * (UAVCAN_NODE_ID_ALLOCATION_MAX_REQUEST_PERIOD_US - UAVCAN_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_US)); } static void allocation_start_followup_timer(void) { if (!allocation_running()) { return; } allocation_state.request_timer_begin_us = micros(); allocation_state.request_delay_us = UAVCAN_NODE_ID_ALLOCATION_MIN_FOLLOWUP_PERIOD_US + (uint32_t)(uavcan_get_radom_float() * (UAVCAN_NODE_ID_ALLOCATION_MAX_FOLLOWUP_PERIOD_US - UAVCAN_NODE_ID_ALLOCATION_MIN_FOLLOWUP_PERIOD_US)); } static bool allocation_running(void) { return canardGetLocalNodeID(&g_canard) == CANARD_BROADCAST_NODE_ID; } static float uavcan_get_radom_float(void) { static bool initialized = false; if (!initialized) { initialized = true; read_unique_id(&node_unique_id[0]); const uint32_t *unique_32 = (uint32_t *)&node_unique_id[0]; srand(micros() ^ *unique_32); } return (float)rand() / (float)RAND_MAX; } void uavcan_init_by_allocation(void) { can_filter_none(); can_init_irq(); canardInit(&g_canard, // Uninitialized library instance g_canard_memory_pool, // Raw memory chunk used for dynamic allocation sizeof(g_canard_memory_pool), // Size of the above, in bytes onTransferReceived, // Callback, see CanardOnTransferReception shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer NULL); can_init_irq(); read_unique_id(&node_unique_id[0]); allocation_init(); // canard_initialized = true; allocation_init_tick = HAL_GetTick(); } uint64_t micros(void) { return HAL_GetTick()*1000 + __HAL_TIM_GetCounter(&htim2); } void init_uavcan_fifo(uint8_t node_id) { uint32_t id = 0; uint32_t mask = 0; uint8_t bank_index = 1; uavcan_get_uavcan_id_mask_by_msg_id(UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID, &id, &mask); can_filter_init_with_mask(bank_index, id, mask, 1); bank_index++; uavcan_get_uavcan_id_mask_by_msg_id(UAVCAN_FILE_READ_DATA_TYPE_ID, &id, &mask); can_filter_init_with_mask(bank_index, id, mask, 0); bank_index++; uavcan_get_uavcan_id_mask_by_msg_id(UAVCAN_RESTART_NODE_DATA_TYPE_ID, &id, &mask); can_filter_init_with_mask(bank_index, id, mask, 1); bank_index++; uavcan_get_uavcan_id_mask_by_node_id(node_id, &id, &mask); can_filter_init_with_mask(bank_index, id, mask, 1); bank_index++; }