proximity_uavcan_converter/UAVCAN/uavcan.c

#include "uavcan.h"
#include <string.h>
#include <stdlib.h>
#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);
    }
}