4d_ObstacleRadarDroneCAN/App/dri_can.c

#include "dri_can.h"
#include "dri_Flash.h"
#include <rthw.h>
#include <rtthread.h>
#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;
}