#include "uavcan.h" #include #include #include "main.h" #include "math.h" #include "node_flow_water.h" #define MEMPOOL_OBJECTS 16 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 const uint8_t gimbal_id = 0xC5; //Arena for memory allocation, used by the library //static uint32_t g_uptime = 0; uint8_t sver_p1 = 1; uint8_t sver_p2 = 0; uint8_t sver_p3 = 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 node_unique_id[UNIQUE_ID_LENGTH_BYTES]; static uint8_t transfer_id = 0; //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; #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 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 ZRZK_EQUIPMENT_FLOW_WATER_ID: *out_data_type_signature = ZRZK_EQUIPMENT_FLOW_WATER_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; 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(&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) { set_unique_id("com.zr3d.proxi"); 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); if (can_setup_filters(canardGetLocalNodeID(&g_canard)) < 0) { can_setup_filters_none(); } } void uavcan_init_with_cubemx(void) { set_unique_id("com.zr3d.proxi"); initCanOnlyFilter(); 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); if (can_setup_filters(canardGetLocalNodeID(&g_canard)) < 0) { can_setup_filters_none(); } init_CAN_IRQ(); } void init_allocation(void) { set_unique_id("com.zr3d.proxi"); initCanOnlyFilter(); 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); // if (can_setup_filters(canardGetLocalNodeID(&g_canard)) < 0) // { // can_setup_filters_none(); // } init_CAN_IRQ(); //执行动态节点ID分配过程 static const uint8_t preferred_node_id = CANARD_BROADCAST_NODE_ID; node_id_allocation_unique_id_offset = 0; uint8_t node_id_allocation_transfer_id = 0; while (canardGetLocalNodeID(&g_canard) == CANARD_BROADCAST_NODE_ID) { //puts("Waiting for dynamic node ID allocation..."); // vTaskDelay(50); send_next_node_id_allocation_request_at = HAL_GetTick() + UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC + (uint64_t)(get_random_float() * UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC); // (uint64_t)(0.1 * UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC); while ((HAL_GetTick() < send_next_node_id_allocation_request_at) && (canardGetLocalNodeID(&g_canard) == CANARD_BROADCAST_NODE_ID)) { send_canard(); receive_canard(); osDelay(5); } if (canardGetLocalNodeID(&g_canard) != CANARD_BROADCAST_NODE_ID) { break; } // Structure of the request is documented in the DSDL definition 请求的结构记录在DSDL定义中 // 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)(preferred_node_id << 1U); if (node_id_allocation_unique_id_offset == 0) { allocation_request[0] |= 1; // First part of unique ID } uint8_t my_unique_id[UNIQUE_ID_LENGTH_BYTES]; get_unique_id(my_unique_id); static const uint8_t max_len_of_unique_id_in_request = 6; uint8_t uid_size = (uint8_t)(UNIQUE_ID_LENGTH_BYTES - node_id_allocation_unique_id_offset); if (uid_size > max_len_of_unique_id_in_request) { uid_size = max_len_of_unique_id_in_request; } // Paranoia time assert(node_id_allocation_unique_id_offset < UNIQUE_ID_LENGTH_BYTES); assert(uid_size <= max_len_of_unique_id_in_request); assert(uid_size > 0); assert((uid_size + node_id_allocation_unique_id_offset) <= UNIQUE_ID_LENGTH_BYTES); memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset], uid_size); // Broadcasting the request const int16_t bcast_res = canardBroadcast(&g_canard, UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE, UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID, &node_id_allocation_transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &allocation_request[0], (uint16_t)(uid_size + 1)); if (bcast_res < 0) { //(void)fprintf(stderr, "Could not broadcast dynamic node ID allocation request; error %d\n", bcast_res); //TODO ////my_printf() } // Preparing for timeout; if response is received, this value will be updated from the callback. node_id_allocation_unique_id_offset = 0; } //过滤设置 if (canardGetLocalNodeID(&g_canard) != 0) //TODO 其实这个判断可以删除，稳妥起见，再判断一次 { if (can_setup_filters(canardGetLocalNodeID(&g_canard)) < 0) { can_setup_filters_none(); } } init_CAN_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); //some indication } 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(); get_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; } //动态节点消息接收处理 //on_allocation_message void handle_allocation_data(CanardInstance *ins, CanardRxTransfer *transfer) { // Rule C - updating the randomized time interval send_next_node_id_allocation_request_at = HAL_GetTick() + UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC + //getMonotonicTimestampUSec() (uint64_t)(get_random_float() * UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC); if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) { // puts("Allocation request from another allocatee"); node_id_allocation_unique_id_offset = 0; return; } //Copying the unique ID from the message unique_id_bit_offset：位偏移量 --uint8 static const uint8_t unique_id_bit_offset = 8; uint8_t received_unique_id[UNIQUE_ID_LENGTH_BYTES]; uint8_t received_unique_id_len = 0; for (; received_unique_id_len < (transfer->payload_len - (unique_id_bit_offset / 8U)); received_unique_id_len++) { assert(received_unique_id_len < UNIQUE_ID_LENGTH_BYTES); const uint8_t bit_offset = (uint8_t)(unique_id_bit_offset + received_unique_id_len * 8U); (void)canardDecodeScalar(transfer, bit_offset, 8, false, &received_unique_id[received_unique_id_len]); } // Obtaining the local unique ID 获取本地 unique ID uint8_t my_unique_id[UNIQUE_ID_LENGTH_BYTES]; get_unique_id(my_unique_id); // Matching the received UID against the local one if (memcmp(received_unique_id, my_unique_id, received_unique_id_len) != 0) { node_id_allocation_unique_id_offset = 0; return; // No match, return } if (received_unique_id_len < UNIQUE_ID_LENGTH_BYTES) { // 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 -= UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC; } else { // Allocation complete - copying the allocated node ID from the message uint8_t allocated_node_id = 0; (void)canardDecodeScalar(transfer, 0, 7, false, &allocated_node_id); assert(allocated_node_id <= 127); canardSetLocalNodeID(ins, allocated_node_id); } } 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 get_unique_id(uint8_t *out_uid) { for (uint8_t i = 0; i < UNIQUE_ID_LENGTH_BYTES; i++) { out_uid[i] = node_unique_id[i]; } } void set_unique_id(char *str_unique_id) { for (uint8_t i = 0; i < strlen(str_unique_id); i++) { node_unique_id[i] = str_unique_id[i]; } } 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 ZRZK_EQUIPMENT_FLOW_WATERDEPTH_ID: // publish_water_dapth(&send_data->content.water_depth_msg); //break; case UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_ID: publish_device_temperature(&send_data->content.device_temp_msg); break; case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID: publish_measurement_canard(&send_data->content.measurement_msg); 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_measurement_canard(uavcan_measurement_t *msg) //MEASUREMENT { uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE]; uint32_t len = uavcan_equipment_range_sensor_Measurement_encode(msg, &buffer[0]); if (UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE != len) return; canardBroadcast(&g_canard, UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID, &transfer_id, CANARD_TRANSFER_PRIORITY_HIGH, &buffer[0], len); // HAL_UART_AbortReceive(&huart1); // HAL_NVIC_DisableIRQ(USART1_IRQn); // send_canard(); // HAL_NVIC_EnableIRQ(USART1_IRQn); } void publish_device_temperature(uavcan_equipment_device_Temperature *data) { uint8_t buffer[UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_MAX_SIZE] = {0}; uint32_t len = uavcan_equipment_device_Temperature_encode(data, &buffer); if (len > 0) { canardBroadcast( &g_canard, UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_SIGNATURE, UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_ID, &transfer_id, CANARD_TRANSFER_PRIORITY_LOWEST, buffer, len); } } 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); } }