#include "dri_can.h" #include "dri_Flash.h" #include #include #include "n32g45x_can.h" #include "n32g45x_gpio.h" #include "string.h" #include "application.h" #include "algorithm.h" uint16_t canBitrate = 6; #define DRONECAN_MEM_ARENA_SIZE 4096U #define DRONECAN_MEM_BLOCK_COUNT (DRONECAN_MEM_ARENA_SIZE / CANARD_MEM_BLOCK_SIZE) #define DRONECAN_RX_QUEUE_SIZE 64U #define DRONECAN_RX_THREAD_STACK_SIZE 2048U #define DRONECAN_RX_THREAD_PRIORITY 3U #define DRONECAN_RX_THREAD_TICK 1U #define DRONECAN_RX_POLL_TICKS ( RT_TICK_PER_SECOND / 20U ) #define DRONECAN_UNIQUE_ID_LENGTH 16U #define DRONECAN_DNA_REQUEST_CHUNK_LENGTH 6U #define DRONECAN_DNA_FIRST_REQUEST_MIN_MS 600U #define DRONECAN_DNA_FIRST_REQUEST_JITTER_MS 400U #define DRONECAN_DNA_FOLLOWUP_DELAY_MS 50U #define DRONECAN_DNA_FOLLOWUP_TIMEOUT_MS UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_FOLLOWUP_TIMEOUT_MS #define DRONECAN_DNA_MAX_TIMEOUTS 5U #define DRONECAN_STATIC_NODE_ID 125U #define DRONECAN_NODE_INFO_NAME "com.vkradar.r4d" #define DRONECAN_NODE_INFO_NAME_LENGTH ( sizeof( DRONECAN_NODE_INFO_NAME ) - 1U ) #define DRONECAN_SOFTWARE_VERSION_MAJOR 5U #define DRONECAN_SOFTWARE_VERSION_MINOR 0U #define DRONECAN_HARDWARE_VERSION_MAJOR 1U #define DRONECAN_HARDWARE_VERSION_MINOR 0U #define DRONECAN_BEGIN_FW_UPDATE_ERROR_MESSAGE "DroneCAN bootloader unavailable" #define DRONECAN_BEGIN_FW_UPDATE_STORAGE_ERROR_MESSAGE "Cannot store update context" #define DRONECAN_CAN_BITRATE 500000U #define FRAME_NUM 5 #define FRAME_DATA_LEN 8 #define VALID_PAYLOAD_LEN (FRAME_NUM * (FRAME_DATA_LEN - 1)) typedef struct { uint8_t buf[VALID_PAYLOAD_LEN]; uint8_t busy; uint8_t done; uint8_t expected_sequence; uint8_t rx_count; }can_rx_longframe; can_rx_longframe can_info_frame = {0}; CanardInstance canard = {0}; static CanardPoolAllocatorBlock canard_memory_pool[DRONECAN_MEM_BLOCK_COUNT] = {0}; static uint8_t transfer_id_global_navigation_solution = 0; static uint8_t transfer_id_range_sensor_measurement = 0; static uint8_t transfer_id_node_status = 0; static uint8_t transfer_id_dynamic_node_id_allocation = 0; static volatile uint8_t dronecan_ready = 0; static struct rt_semaphore dronecan_rx_sem; static struct rt_thread dronecan_rx_thread; static struct rt_mutex dronecan_tx_lock; static volatile uint8_t dronecan_tx_lock_ready = 0; ALIGN( RT_ALIGN_SIZE ) static rt_uint8_t dronecan_rx_stack[DRONECAN_RX_THREAD_STACK_SIZE]; static CanRxMessage dronecan_rx_queue[DRONECAN_RX_QUEUE_SIZE]; static volatile uint8_t dronecan_rx_head = 0; static volatile uint8_t dronecan_rx_tail = 0; static volatile uint8_t dronecan_rx_count = 0; static volatile uint8_t dronecan_rx_started = 0; static volatile uint32_t dronecan_rx_overflow = 0; static uint8_t dronecan_unique_id[DRONECAN_UNIQUE_ID_LENGTH] = {0}; static uint8_t dronecan_dna_request_offset = 0; static uint8_t dronecan_dna_expected_uid_length = 0; static uint8_t dronecan_dna_waiting_for_response = 0; static uint8_t dronecan_dna_retry_count = 0; static uint8_t dronecan_dna_timeout_count = 0; static rt_tick_t dronecan_dna_next_request_tick = 0; static rt_tick_t dronecan_dna_response_deadline_tick = 0; extern float q_now[4]; extern float p_now[3]; extern uint8_t other[4]; extern int16_t euler[3]; extern uint8_t ParameterData[8]; extern rt_sem_t parameter_sem_t; static uint64_t DroneCAN_GetTimeUsec( void ) { return ( ( uint64_t )rt_tick_get() * 1000000ULL ) / ( uint64_t )RT_TICK_PER_SECOND; } void FLASH_LOG( void ); static void DroneCAN_FlushTxQueue( void ); static rt_tick_t DroneCAN_MillisecondsToTicks( uint32_t milliseconds ) { return ( rt_tick_t )( ( milliseconds * RT_TICK_PER_SECOND + 999U ) / 1000U ); } static bool DroneCAN_TickReached( rt_tick_t now, rt_tick_t deadline ) { return ( ( int32_t )( now - deadline ) >= 0 ); } static bool DroneCAN_TxLock( void ) { return ( dronecan_tx_lock_ready != 0 && rt_mutex_take( &dronecan_tx_lock, RT_WAITING_FOREVER ) == RT_EOK ); } static void DroneCAN_TxUnlock( void ) { rt_mutex_release( &dronecan_tx_lock ); } bool DroneCAN_IsNodeIDAllocated( void ) { return ( dronecan_ready != 0 && canardGetLocalNodeID( &canard ) != CANARD_BROADCAST_NODE_ID ); } static bool DroneCAN_ShouldAccept( 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 ( transfer_type == CanardTransferTypeBroadcast && data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID && canardGetLocalNodeID( ins ) == CANARD_BROADCAST_NODE_ID ) { *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE; return true; } if ( transfer_type == CanardTransferTypeBroadcast && data_type_id == UAVCAN_NAVIGATION_GLOBALNAVIGATIONSOLUTION_ID ) { *out_data_type_signature = UAVCAN_NAVIGATION_GLOBALNAVIGATIONSOLUTION_SIGNATURE; return true; } if ( transfer_type == CanardTransferTypeRequest && data_type_id == UAVCAN_PROTOCOL_GETNODEINFO_ID && canardGetLocalNodeID( ins ) != CANARD_BROADCAST_NODE_ID ) { *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE; return true; } if ( transfer_type == CanardTransferTypeRequest && data_type_id == UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID && canardGetLocalNodeID( ins ) != CANARD_BROADCAST_NODE_ID ) { *out_data_type_signature = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE; return true; } return false; } static bool DroneCAN_IsRelevantFrame( const CanRxMessage *rx_msg ) { uint32_t ext_id; if ( rx_msg == RT_NULL || rx_msg->IDE != CAN_Extended_Id || rx_msg->RTR != CAN_RTRQ_Data || rx_msg->DLC == 0 ) { return false; } ext_id = rx_msg->ExtId; if ( ( ( ext_id >> 7U ) & 0x1U ) == 0U ) { const uint16_t data_type_id = ( uint16_t )( ( ext_id >> 8U ) & 0xFFFFU ); return ( data_type_id == UAVCAN_NAVIGATION_GLOBALNAVIGATIONSOLUTION_ID || data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID ); } return ( DroneCAN_IsNodeIDAllocated() && ( ( ( uint8_t )( ( ext_id >> 16U ) & 0xFFU ) == UAVCAN_PROTOCOL_GETNODEINFO_ID ) || ( ( uint8_t )( ( ext_id >> 16U ) & 0xFFU ) == UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID ) ) && ( ( ( ext_id >> 15U ) & 0x1U ) != 0U ) && ( ( uint8_t )( ( ext_id >> 8U ) & 0x7FU ) == canardGetLocalNodeID( &canard ) ) ); } static void DroneCAN_DNA_Reset( rt_tick_t base_tick ) { const uint32_t jitter = ( uint32_t )( dronecan_unique_id[0] + dronecan_unique_id[15] + dronecan_dna_retry_count * 37U ) % ( DRONECAN_DNA_FIRST_REQUEST_JITTER_MS + 1U ); dronecan_dna_request_offset = 0; dronecan_dna_expected_uid_length = 0; dronecan_dna_waiting_for_response = 0; dronecan_dna_retry_count++; dronecan_dna_next_request_tick = base_tick + DroneCAN_MillisecondsToTicks( DRONECAN_DNA_FIRST_REQUEST_MIN_MS + jitter ); } static bool DroneCAN_SendDynamicNodeIDRequest( uint8_t uid_offset ) { struct uavcan_protocol_dynamic_node_id_Allocation request = {0}; uint8_t buffer[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_SIZE] = {0}; uint8_t uid_length; uint32_t length; int16_t result; if ( uid_offset >= DRONECAN_UNIQUE_ID_LENGTH || DroneCAN_IsNodeIDAllocated() ) { return false; } uid_length = DRONECAN_UNIQUE_ID_LENGTH - uid_offset; if ( uid_length > DRONECAN_DNA_REQUEST_CHUNK_LENGTH ) { uid_length = DRONECAN_DNA_REQUEST_CHUNK_LENGTH; } request.node_id = CANARD_BROADCAST_NODE_ID; request.first_part_of_unique_id = ( uid_offset == 0U ); request.unique_id.len = uid_length; memcpy( request.unique_id.data, &dronecan_unique_id[uid_offset], uid_length ); length = uavcan_protocol_dynamic_node_id_Allocation_encode( &request, buffer ); if ( DroneCAN_TxLock() == false ) { return false; } result = canardBroadcast( &canard, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, &transfer_id_dynamic_node_id_allocation, CANARD_TRANSFER_PRIORITY_LOW, buffer, ( uint16_t )length ); if ( result > 0 ) { DroneCAN_FlushTxQueue(); } DroneCAN_TxUnlock(); return ( result > 0 ); } static void DroneCAN_SendInitialNodeStatus( void ) { struct uavcan_protocol_NodeStatus status = {0}; status.uptime_sec = rt_tick_get() / RT_TICK_PER_SECOND; status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; DroneCAN_SendNodeStatus( &status ); } static void DroneCAN_UseStaticNodeID( void ) { if ( DroneCAN_IsNodeIDAllocated() || DRONECAN_STATIC_NODE_ID < CANARD_MIN_NODE_ID || DRONECAN_STATIC_NODE_ID > CANARD_MAX_NODE_ID ) { return; } canardSetLocalNodeID( &canard, DRONECAN_STATIC_NODE_ID ); dronecan_dna_waiting_for_response = 0; dronecan_dna_request_offset = DRONECAN_UNIQUE_ID_LENGTH; dronecanUpdateInfo.local_node_id = DRONECAN_STATIC_NODE_ID; // write_dev_info(); DroneCAN_SendInitialNodeStatus(); } static void DroneCAN_HandleDynamicNodeIDAllocation( CanardRxTransfer *transfer ) { struct uavcan_protocol_dynamic_node_id_Allocation response = {0}; rt_tick_t now = rt_tick_get(); if ( DroneCAN_IsNodeIDAllocated() || uavcan_protocol_dynamic_node_id_Allocation_decode( transfer, &response ) != false ) { return; } /* Another anonymous requester interrupts the allocator's UID session. */ if ( transfer->source_node_id == CANARD_BROADCAST_NODE_ID ) { DroneCAN_DNA_Reset( now ); return; } if ( dronecan_dna_waiting_for_response == 0 || response.unique_id.len != dronecan_dna_expected_uid_length || memcmp( response.unique_id.data, dronecan_unique_id, response.unique_id.len ) != 0 ) { DroneCAN_DNA_Reset( now ); return; } if ( response.unique_id.len == DRONECAN_UNIQUE_ID_LENGTH ) { if ( response.node_id < CANARD_MIN_NODE_ID || response.node_id > CANARD_MAX_NODE_ID ) { DroneCAN_DNA_Reset( now ); return; } canardSetLocalNodeID( &canard, response.node_id ); dronecan_dna_waiting_for_response = 0; dronecan_dna_request_offset = DRONECAN_UNIQUE_ID_LENGTH; dronecan_dna_timeout_count = 0; // devInfo.nodeId = response.node_id; // write_dev_info(); dronecanUpdateInfo.local_node_id = response.node_id; DroneCAN_SendInitialNodeStatus(); return; } if ( response.node_id != CANARD_BROADCAST_NODE_ID ) { DroneCAN_DNA_Reset( now ); return; } dronecan_dna_request_offset = response.unique_id.len; dronecan_dna_waiting_for_response = 0; dronecan_dna_timeout_count = 0; dronecan_dna_next_request_tick = now + DroneCAN_MillisecondsToTicks( DRONECAN_DNA_FOLLOWUP_DELAY_MS ); } static void DroneCAN_SendGetNodeInfoResponse( CanardRxTransfer *request ) { struct uavcan_protocol_GetNodeInfoResponse response = {0}; uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE] = {0}; uint32_t length; response.status.uptime_sec = rt_tick_get() / RT_TICK_PER_SECOND; response.status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; response.status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; response.software_version.major = DRONECAN_SOFTWARE_VERSION_MAJOR; response.software_version.minor = DRONECAN_SOFTWARE_VERSION_MINOR; response.hardware_version.major = DRONECAN_HARDWARE_VERSION_MAJOR; response.hardware_version.minor = DRONECAN_HARDWARE_VERSION_MINOR; memcpy( response.hardware_version.unique_id, dronecan_unique_id, sizeof( dronecan_unique_id ) ); response.name.len = DRONECAN_NODE_INFO_NAME_LENGTH; memcpy( response.name.data, DRONECAN_NODE_INFO_NAME, response.name.len ); length = uavcan_protocol_GetNodeInfoResponse_encode( &response, buffer ); if ( DroneCAN_TxLock() == false ) { return; } canardReleaseRxTransferPayload( &canard, request ); if ( canardRequestOrRespond( &canard, request->source_node_id, UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, UAVCAN_PROTOCOL_GETNODEINFO_ID, &request->transfer_id, request->priority, CanardResponse, buffer, ( uint16_t )length ) > 0 ) { DroneCAN_FlushTxQueue(); } DroneCAN_TxUnlock(); } static bool DroneCAN_SaveBeginFirmwareUpdateRequest( const struct uavcan_protocol_file_BeginFirmwareUpdateRequest *request ) { const uint8_t path_length = request->image_file_remote_path.path.len; if ( request->source_node_id < CANARD_MIN_NODE_ID || request->source_node_id > CANARD_MAX_NODE_ID || path_length == 0U || path_length > DRONECAN_FILE_PATH_MAX_LENGTH ) { return false; } // memset( &dronecanUpdateInfo, 0, sizeof( dronecanUpdateInfo ) ); dronecanUpdateInfo.magic = DRONECAN_UPDATE_CONTEXT_MAGIC; dronecanUpdateInfo.version = DRONECAN_UPDATE_CONTEXT_VERSION; dronecanUpdateInfo.size = sizeof( dronecanUpdateInfo ); dronecanUpdateInfo.state = DRONECAN_UPDATE_STATE_PENDING; dronecanUpdateInfo.local_node_id = canardGetLocalNodeID( &canard ); dronecanUpdateInfo.file_server_node_id = request->source_node_id; dronecanUpdateInfo.can_bitrate = DRONECAN_CAN_BITRATE; dronecanUpdateInfo.path_length = path_length; dronecanUpdateInfo.upgradeEable = 1; memcpy( dronecanUpdateInfo.path, request->image_file_remote_path.path.data, path_length ); dronecanUpdateInfo.crc32 = DroneCAN_UpdateContextCalcCRC32( &dronecanUpdateInfo ); return DroneCAN_UpdateContextWrite() == 0; } static void DroneCAN_SendBeginFirmwareUpdateResponse( CanardRxTransfer *request, uint8_t error, const char *error_message ) { struct uavcan_protocol_file_BeginFirmwareUpdateResponse response = {0}; uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE] = {0}; size_t error_message_length = 0U; uint32_t length; response.error = error; if ( error_message != RT_NULL ) { error_message_length = strlen( error_message ); if ( error_message_length > sizeof( response.optional_error_message.data ) ) { error_message_length = sizeof( response.optional_error_message.data ); } response.optional_error_message.len = ( uint8_t )error_message_length; memcpy( response.optional_error_message.data, error_message, error_message_length ); } length = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode( &response, buffer ); if ( DroneCAN_TxLock() == false ) { return; } canardReleaseRxTransferPayload( &canard, request ); if ( canardRequestOrRespond( &canard, request->source_node_id, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, &request->transfer_id, request->priority, CanardResponse, buffer, ( uint16_t )length ) > 0 ) { DroneCAN_FlushTxQueue(); } DroneCAN_TxUnlock(); } static void DroneCAN_OnReception( CanardInstance *ins, CanardRxTransfer *transfer ) { ( void )ins; if ( transfer->transfer_type == CanardTransferTypeBroadcast && transfer->data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID ) { DroneCAN_HandleDynamicNodeIDAllocation( transfer ); return; } if ( transfer->transfer_type == CanardTransferTypeRequest && transfer->data_type_id == UAVCAN_PROTOCOL_GETNODEINFO_ID && DroneCAN_IsNodeIDAllocated() ) { struct uavcan_protocol_GetNodeInfoRequest request; if ( uavcan_protocol_GetNodeInfoRequest_decode( transfer, &request ) == false ) { DroneCAN_SendGetNodeInfoResponse( transfer ); } return; } if ( transfer->transfer_type == CanardTransferTypeRequest && transfer->data_type_id == UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID && DroneCAN_IsNodeIDAllocated() ) { struct uavcan_protocol_file_BeginFirmwareUpdateRequest request; if ( uavcan_protocol_file_BeginFirmwareUpdateRequest_decode( transfer, &request ) == false ) { if ( DroneCAN_SaveBeginFirmwareUpdateRequest( &request ) ) { /* Download/restart support is added with the DroneCAN bootloader. */ DroneCAN_SendBeginFirmwareUpdateResponse( transfer, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK, DRONECAN_BEGIN_FW_UPDATE_ERROR_MESSAGE ); // FLASH_LOG(); __set_PRIMASK( 1 ); NVIC_SystemReset(); } else { DroneCAN_SendBeginFirmwareUpdateResponse( transfer, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_UNKNOWN, DRONECAN_BEGIN_FW_UPDATE_STORAGE_ERROR_MESSAGE ); } } return; } if ( transfer->transfer_type == CanardTransferTypeBroadcast && transfer->data_type_id == UAVCAN_NAVIGATION_GLOBALNAVIGATIONSOLUTION_ID ) { static struct uavcan_navigation_GlobalNavigationSolution msg; if ( uavcan_navigation_GlobalNavigationSolution_decode( transfer, &msg ) == false ) { rt_base_t level = rt_hw_interrupt_disable(); q_now[0] = msg.orientation_xyzw[3]; q_now[1] = msg.orientation_xyzw[0]; q_now[2] = msg.orientation_xyzw[1]; q_now[3] = msg.orientation_xyzw[2]; p_now[0] = ( float )msg.longitude; p_now[1] = ( float )msg.latitude; p_now[2] = msg.height_msl; rt_hw_interrupt_enable( level ); } } } static volatile int16_t dronecan_last_ret; static uint32_t dronecanCanardFalse = 0; static void DroneCAN_HandleRxMessage( const CanRxMessage *rx_msg ) { CanardCANFrame rx_frame; uint8_t data_len; if ( dronecan_ready == 0 || DroneCAN_IsRelevantFrame( rx_msg ) == false ) { return; } data_len = rx_msg->DLC; if ( data_len > CANARD_CAN_FRAME_MAX_DATA_LEN ) { data_len = CANARD_CAN_FRAME_MAX_DATA_LEN; } memset( &rx_frame, 0, sizeof( rx_frame ) ); rx_frame.id = ( rx_msg->ExtId & CANARD_CAN_EXT_ID_MASK ) | CANARD_CAN_FRAME_EFF; rx_frame.data_len = data_len; rx_frame.iface_id = 0; memcpy( rx_frame.data, rx_msg->Data, data_len ); dronecan_last_ret = canardHandleRxFrame( &canard, &rx_frame, DroneCAN_GetTimeUsec() ); if (dronecan_last_ret != CANARD_OK) dronecanCanardFalse++; } static void DroneCAN_RxQueuePushFromIsr( const CanRxMessage *rx_msg ) { if ( dronecan_rx_started == 0 || DroneCAN_IsRelevantFrame( rx_msg ) == false ) { return; } if ( dronecan_rx_count >= DRONECAN_RX_QUEUE_SIZE ) { dronecan_rx_overflow++; return; } dronecan_rx_queue[dronecan_rx_head] = *rx_msg; dronecan_rx_head = ( uint8_t )( ( dronecan_rx_head + 1U ) % DRONECAN_RX_QUEUE_SIZE ); if ( dronecan_rx_count++ == 0 ) { rt_sem_release( &dronecan_rx_sem ); } } static bool DroneCAN_RxQueuePop( CanRxMessage *rx_msg ) { rt_base_t level; if ( rx_msg == RT_NULL ) { return false; } level = rt_hw_interrupt_disable(); if ( dronecan_rx_count == 0 ) { rt_hw_interrupt_enable( level ); return false; } *rx_msg = dronecan_rx_queue[dronecan_rx_tail]; dronecan_rx_tail = ( uint8_t )( ( dronecan_rx_tail + 1U ) % DRONECAN_RX_QUEUE_SIZE ); dronecan_rx_count--; rt_hw_interrupt_enable( level ); return true; } static void DroneCAN_RxThreadEntry( void *parameter ) { CanRxMessage rx_msg; rt_tick_t last_cleanup_tick = rt_tick_get(); ( void )parameter; while ( 1 ) { while ( DroneCAN_RxQueuePop( &rx_msg ) == true ) { DroneCAN_HandleRxMessage( &rx_msg ); } if ( DroneCAN_IsNodeIDAllocated() == false ) { const rt_tick_t now = rt_tick_get(); if ( dronecan_dna_waiting_for_response != 0 ) { if ( DroneCAN_TickReached( now, dronecan_dna_response_deadline_tick ) ) { dronecan_dna_timeout_count++; if ( dronecan_dna_timeout_count >= DRONECAN_DNA_MAX_TIMEOUTS ) { DroneCAN_UseStaticNodeID(); } else { DroneCAN_DNA_Reset( now ); } } } else if ( DroneCAN_TickReached( now, dronecan_dna_next_request_tick ) && DroneCAN_SendDynamicNodeIDRequest( dronecan_dna_request_offset ) ) { uint8_t uid_length = DRONECAN_UNIQUE_ID_LENGTH - dronecan_dna_request_offset; if ( uid_length > DRONECAN_DNA_REQUEST_CHUNK_LENGTH ) { uid_length = DRONECAN_DNA_REQUEST_CHUNK_LENGTH; } dronecan_dna_expected_uid_length = dronecan_dna_request_offset + uid_length; dronecan_dna_waiting_for_response = 1; dronecan_dna_response_deadline_tick = now + DroneCAN_MillisecondsToTicks( DRONECAN_DNA_FOLLOWUP_TIMEOUT_MS ); } else if ( DroneCAN_TickReached( now, dronecan_dna_next_request_tick ) ) { DroneCAN_DNA_Reset( now ); } } if ( ( rt_tick_get() - last_cleanup_tick ) >= RT_TICK_PER_SECOND ) { canardCleanupStaleTransfers( &canard, DroneCAN_GetTimeUsec() ); last_cleanup_tick = rt_tick_get(); } rt_sem_take( &dronecan_rx_sem, DRONECAN_RX_POLL_TICKS ); } } const uint8_t auchCRCHi[] = { 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40 }; const uint8_t auchCRCLo[] = { 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40 }; void can_init( void ) { GPIO_InitType GPIO_InitStructure; CAN_InitType CAN_InitStructure; CAN_FilterInitType CAN_FilterInitStructure; RCC_EnableAPB2PeriphClk( RCC_APB2_PERIPH_AFIO | RCC_APB2_PERIPH_GPIOB | RCC_APB2_PERIPH_GPIOA, ENABLE ); RCC_EnableAPB1PeriphClk( RCC_APB1_PERIPH_CAN1, ENABLE ); GPIO_InitStructure.Pin = GPIO_PIN_11; //GPIO_PIN_8; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; GPIO_InitPeripheral( GPIOA, &GPIO_InitStructure ); GPIO_InitStructure.Pin = GPIO_PIN_12;//GPIO_PIN_9; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitPeripheral( GPIOA, &GPIO_InitStructure ); GPIO_ConfigPinRemap( GPIO_RMP0_CAN1, ENABLE ); CAN_DeInit( CAN1 ); /* Struct init*/ CAN_InitStruct( &CAN_InitStructure ); CAN_InitStructure.ABOM = ENABLE; CAN_InitStructure.AWKUM = ENABLE; CAN_InitStructure.OperatingMode = CAN_Normal_Mode; CAN_InitStructure.NART = ENABLE; CAN_InitStructure.RFLM = ENABLE; CAN_InitStructure.TTCM = DISABLE; CAN_InitStructure.TXFP = ENABLE; CAN_InitStructure.TBS1 = CAN_TBS1_3tq; CAN_InitStructure.TBS2 = CAN_TBS2_2tq; CAN_InitStructure.RSJW = CAN_RSJW_1tq; CAN_InitStructure.BaudRatePrescaler = canBitrate; CAN_Init( CAN1, &CAN_InitStructure ); CAN_FilterInitStructure.Filter_FIFOAssignment = CAN_Filter_FIFO0; CAN_FilterInitStructure.Filter_Mode = CAN_Filter_IdMaskMode; CAN_FilterInitStructure.Filter_Num = 0; CAN_FilterInitStructure.Filter_Scale = CAN_Filter_32bitScale; CAN_FilterInitStructure.FilterMask_HighId = 0X0000; CAN_FilterInitStructure.FilterMask_LowId = 0X0000; CAN_FilterInitStructure.Filter_HighId = 0X0000; CAN_FilterInitStructure.Filter_LowId = 0X0000; CAN_FilterInitStructure.Filter_Act = ENABLE; CAN1_InitFilter( &CAN_FilterInitStructure ); CAN_INTConfig( CAN1, CAN_INT_FMP0, ENABLE ); NVIC_InitType NVIC_InitStructure; NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init( &NVIC_InitStructure ); canardInit( &canard, canard_memory_pool, sizeof( canard_memory_pool ), DroneCAN_OnReception, DroneCAN_ShouldAccept, RT_NULL ); read_uid( uid ); memcpy( dronecan_unique_id, uid, sizeof( uid ) ); dronecan_unique_id[12] = 'V'; dronecan_unique_id[13] = 'K'; dronecan_unique_id[14] = '4'; dronecan_unique_id[15] = 'D'; dronecan_ready = 1; } void DroneCAN_Start( void ) { rt_err_t result; if ( dronecan_rx_started != 0 || dronecan_ready == 0 ) { return; } rt_sem_init( &dronecan_rx_sem, "dc_rx", 0, RT_IPC_FLAG_FIFO ); if ( rt_mutex_init( &dronecan_tx_lock, "dc_tx", RT_IPC_FLAG_PRIO ) != RT_EOK ) { return; } dronecan_tx_lock_ready = 1; result = rt_thread_init( &dronecan_rx_thread, "dronecan", DroneCAN_RxThreadEntry, RT_NULL, dronecan_rx_stack, sizeof( dronecan_rx_stack ), DRONECAN_RX_THREAD_PRIORITY, DRONECAN_RX_THREAD_TICK ); if ( result != RT_EOK ) { return; } dronecan_rx_head = 0; dronecan_rx_tail = 0; dronecan_rx_count = 0; dronecan_rx_overflow = 0; dronecan_rx_started = 1; DroneCAN_DNA_Reset( rt_tick_get() ); rt_thread_startup( &dronecan_rx_thread ); } static void DroneCAN_FlushTxQueue( void ) { CanardCANFrame *txf = RT_NULL; while ( ( txf = canardPeekTxQueue( &canard ) ) != RT_NULL ) { MyCAN_Transmit( txf->id & CANARD_CAN_EXT_ID_MASK, txf->data_len, txf->data, 0 ); canardPopTxQueue( &canard ); } } void DroneCAN_SendRangeSensorMeasurement( struct uavcan_equipment_range_sensor_Measurement *msg ) { uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE]; uint32_t len; if ( DroneCAN_IsNodeIDAllocated() == false || msg == RT_NULL || DroneCAN_TxLock() == false ) { return; } #if CANARD_ENABLE_TAO_OPTION len = uavcan_equipment_range_sensor_Measurement_encode( msg, buffer, true ); #else len = uavcan_equipment_range_sensor_Measurement_encode( msg, buffer ); #endif if ( canardBroadcast( &canard, UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID, &transfer_id_range_sensor_measurement, CANARD_TRANSFER_PRIORITY_LOW, buffer, ( uint16_t )len ) > 0 ) { DroneCAN_FlushTxQueue(); } DroneCAN_TxUnlock(); } void DroneCAN_SendNodeStatus(struct uavcan_protocol_NodeStatus *msg) { uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE] = {0}; uint32_t len; if ( DroneCAN_IsNodeIDAllocated() == false || msg == RT_NULL || DroneCAN_TxLock() == false ) { return; } len = uavcan_protocol_NodeStatus_encode(msg, buffer); if (canardBroadcast(&canard, UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, UAVCAN_PROTOCOL_NODESTATUS_ID, &transfer_id_node_status, CANARD_TRANSFER_PRIORITY_LOW, buffer, len) > 0) { DroneCAN_FlushTxQueue(); } DroneCAN_TxUnlock(); } void myflash_erasepage( uint32_t addr ) //擦除 { uint8_t retryCount = 0; FLASH_STS sts; rt_base_t level1; level1 = rt_hw_interrupt_disable(); FLASH_Unlock(); while ( 1 ) { sts = FLASH_EraseOnePage( addr ); if ( sts == FLASH_COMPL ) { break; } else { if ( retryCount++ > 3 ) { FLASH_Lock(); rt_hw_interrupt_enable( level1 ); return; } } } FLASH_Lock(); rt_hw_interrupt_enable( level1 ); return; } IAP_INFO iap_def = { .ValidFlag = IAP_VFLAG, .UpgradeFlag = IAP_FLAG }; void FLASH_LOG( void ) { myflash_erasepage( IAP_FLAG ); rt_base_t level1; level1 = rt_hw_interrupt_disable(); FLASH_Unlock(); flash_write( IAP_FLAG_ADDR, ( uint8_t * )&iap_def, sizeof( iap_def ) ); FLASH_Lock(); rt_hw_interrupt_enable( level1 ); } FlagUnion ReceiveFlag = {0}; void USB_LP_CAN1_RX0_IRQHandler( void ) { CanRxMessage RxMessage; rt_interrupt_enter(); if ( CAN_GetIntStatus( CAN1, CAN_INT_FMP0 ) != RESET ) { CAN_ReceiveMessage( CAN1, CAN_FIFO0, &RxMessage ); if ( RxMessage.IDE == CAN_Extended_Id && RxMessage.RTR == CAN_RTRQ_Data ) { if ( DroneCAN_IsRelevantFrame( &RxMessage ) == true ) { DroneCAN_RxQueuePushFromIsr( &RxMessage ); } // if (( ( uint16_t )( ( RxMessage.ExtId >> 8U ) & 0xFFFFU ) == // UAVCAN_NAVIGATION_GLOBALNAVIGATIONSOLUTION_ID )) // { // DroneCAN_RxQueuePushFromIsr( &RxMessage ); // } switch ( RxMessage.ExtId ) { case 0x00002345: // q0 q1 memcpy( &q_now[0], &RxMessage.Data[0], 4 ); memcpy( &q_now[1], &RxMessage.Data[4], 4 ); break; case 0x00002346: // q2 q3 memcpy( &q_now[2], &RxMessage.Data[0], 4 ); memcpy( &q_now[3], &RxMessage.Data[4], 4 ); break; case 0x00002347: // p0 p1 memcpy( &p_now[0], &RxMessage.Data[0], 4 ); memcpy( &p_now[1], &RxMessage.Data[4], 4 ); break; case 0x00002348: // p2 p3 memcpy( &p_now[2], &RxMessage.Data[0], 4 ); memcpy( &other[0], &RxMessage.Data[4], 4 ); break; case 0xA01309: //行业协议 ReceiveFlag.u8flag = RxMessage.Data[7]; if(!can_info_frame.busy){ if(ReceiveFlag.flags.seq != 1) break; can_info_frame.busy = 1; can_info_frame.expected_sequence = 1; can_info_frame.rx_count = 0; } if (ReceiveFlag.flags.seq != can_info_frame.expected_sequence){ can_info_frame.busy = 0; can_info_frame.expected_sequence = 1; can_info_frame.rx_count = 0; break; } memcpy (&can_info_frame.buf[can_info_frame.rx_count * (FRAME_DATA_LEN - 1)] , &RxMessage.Data[0] , FRAME_DATA_LEN - 1); can_info_frame.expected_sequence++; can_info_frame.rx_count++; if(can_info_frame.rx_count >= FRAME_NUM) { can_info_frame.busy = 0; can_info_frame.rx_count = 0; memcpy(q_now, &can_info_frame.buf[0], 16); memcpy(p_now, &can_info_frame.buf[16], 12); memcpy(other, &can_info_frame.buf[28], 4); } break; case 0x381400: if ( confInfo.DeviceType == FRONT_OBSTACLE_RADAR ) { if ( RxMessage.Data[5] == 0x44 && RxMessage.Data[6] == 0x34 && RxMessage.Data[7] == 0x46 ) { FLASH_LOG(); __set_PRIMASK( 1 ); NVIC_SystemReset(); } } if ( confInfo.DeviceType == REAR_OBSTACLE_RADAR ) { if ( RxMessage.Data[5] == 0x44 && RxMessage.Data[6] == 0x34 && RxMessage.Data[7] == 0x42 ) { FLASH_LOG(); __set_PRIMASK( 1 ); NVIC_SystemReset(); } } if ( confInfo.DeviceType == LEFT_OBSTACLE_RADAR ) { if ( RxMessage.Data[5] == 0x44 && RxMessage.Data[6] == 0x34 && RxMessage.Data[7] == 0x4C ) { FLASH_LOG(); __set_PRIMASK( 1 ); NVIC_SystemReset(); } } if ( confInfo.DeviceType == RIGHT_OBSTACLE_RADAR ) { if ( RxMessage.Data[5] == 0x44 && RxMessage.Data[6] == 0x34 && RxMessage.Data[7] == 0x52 ) { FLASH_LOG(); __set_PRIMASK( 1 ); NVIC_SystemReset(); } } case 0xA81300: memcpy( &ParameterData[0], &RxMessage.Data[0], 8 ); memcpy( &RceveiveCanID, &RxMessage.ExtId, sizeof(RxMessage.ExtId) ); rt_sem_release(parameter_sem_t); break; case 0xB81300: memcpy( &ParameterData[0], &RxMessage.Data[0], 8 ); memcpy( &RceveiveCanID, &RxMessage.ExtId, sizeof(RxMessage.ExtId) ); rt_sem_release(parameter_sem_t); break; case 0xC81300: memcpy( &ParameterData[0], &RxMessage.Data[0], 8 ); memcpy( &RceveiveCanID, &RxMessage.ExtId, sizeof(RxMessage.ExtId) ); rt_sem_release(parameter_sem_t); break; case 0xD81300: memcpy( &ParameterData[0], &RxMessage.Data[0], 8 ); memcpy( &RceveiveCanID, &RxMessage.ExtId, sizeof(RxMessage.ExtId) ); rt_sem_release(parameter_sem_t); break; default: break; } } } rt_interrupt_leave(); } void MyCAN_Transmit( uint32_t ID, uint8_t Length, uint8_t *Data, uint8_t sendbit ) { CanTxMessage TxMessage; TxMessage.DLC = Length; TxMessage.ExtId = ID; TxMessage.IDE = CAN_Extended_Id; TxMessage.RTR = CAN_RTRQ_Data; TxMessage.StdId = ID; for ( uint8_t i = 0; i < Length; i++ ) { TxMessage.Data[i] = Data[i]; } uint8_t TransmitMailbox = CAN_TransmitMessage( CAN1, &TxMessage ); uint32_t i = 0XFFF; while ( CAN_TxSTS_Ok != CAN_TransmitSTS( CAN1, TransmitMailbox ) && i > 0 ) { uint8_t status = CAN_GetLastErrCode( CAN1 ); i--; } } void MyCAN_Transmitbeg( uint32_t ID, uint8_t Length, uint8_t *Data ) { // uint8_t a =0,b=0; CanTxMessage TxMessagebeg; TxMessagebeg.DLC = Length; TxMessagebeg.ExtId = ID; TxMessagebeg.IDE = CAN_Extended_Id; TxMessagebeg.RTR = CAN_RTRQ_Data; TxMessagebeg.StdId = ID; for ( uint8_t i = 0; i < Length; i++ ) { TxMessagebeg.Data[i] = Data[i]; } uint8_t TransmitMailbox = CAN_TransmitMessage( CAN1, &TxMessagebeg ); // uint32_t send_time = 0; uint32_t i = 0XFFF; while ( CAN_TxSTS_Ok != CAN_TransmitSTS( CAN1, TransmitMailbox ) && i > 0 ) { i--; } // while (CAN_TransmitSTS(CAN1, CAN_TransmitMessage(CAN1, &TxMessage)) != CAN_TxSTS_Ok); } void MyCAN_Transmitend( uint32_t ID, uint8_t Length, uint8_t *Data, uint8_t sendbit ) { uint8_t z; CanTxMessage TxMessageend; TxMessageend.DLC = 8; TxMessageend.ExtId = ID; TxMessageend.IDE = CAN_Extended_Id; TxMessageend.RTR = CAN_RTRQ_Data; TxMessageend.StdId = ID; for ( z = 0; z < Length; z++ ) { TxMessageend.Data[z] = Data[z]; } for ( z = Length; z < 7; z++ ) { TxMessageend.Data[z] = 0x00; } TxMessageend.Data[z] = sendbit; uint8_t TransmitMailbox = CAN_TransmitMessage( CAN1, &TxMessageend ); uint32_t i = 0XFFF; while ( CAN_TxSTS_Ok != CAN_TransmitSTS( CAN1, TransmitMailbox ) && i > 0 ) { i--; } } uint16_t Get_Crc16( uint8_t *puchMsg, uint16_t usDataLen ) { uint8_t uchCRCHi = 0xFF; // ��CRC �ֽڳ�ʼ�� uint8_t uchCRCLo = 0xFF; // ��CRC �ֽڳ�ʼ�� uint32_t uIndex; // CRC ѭ���е����� while ( usDataLen-- ) // ������Ϣ������ { uIndex = uchCRCLo ^ *puchMsg++; // ����CRC uchCRCLo = uchCRCHi ^ auchCRCHi[uIndex]; uchCRCHi = auchCRCLo[uIndex]; } return ( uchCRCHi << 8 | uchCRCLo ); } uint8_t MyCAN_ReceiveFlag( void ) { if ( CAN_PendingMessage( CAN1, CAN_FIFO0 ) > 0 ) { return 1; } return 0; }