当前位置： 首页 > article >正文

UAVCAN/DroneCAN链路开发

article 2025/1/1 0:18:01

在这里插入图片描述

UAVCAN/DroneCAN是基于CAN通讯上的一种封装协议，常用于无人机飞行器领域。

  • CAN外设初始化，设置滤波器接收指定ID到FIFO0
void can_configuration(void)
{
  can_base_type can_base_struct;
  can_baudrate_type can_baudrate_struct;
  can_filter_init_type can_filter_init_struct;

  if(crm_flag_get(CRM_HEXT_STABLE_FLAG) != SET)
  {
    return ERROR;
  }
  
  /* enable the can clock */
  crm_periph_clock_enable(CRM_CAN1_PERIPH_CLOCK, TRUE);

  /* can base init */
  can_default_para_init(&can_base_struct);
  can_base_struct.mode_selection = CAN_MODE_COMMUNICATE;
  can_base_struct.ttc_enable = FALSE;
  can_base_struct.aebo_enable = TRUE;
  can_base_struct.aed_enable = TRUE;
  can_base_struct.prsf_enable = FALSE;
  can_base_struct.mdrsel_selection = CAN_DISCARDING_FIRST_RECEIVED;
  can_base_struct.mmssr_selection = CAN_SENDING_BY_ID;
  can_base_init(CAN1, &can_base_struct);

  /* can baudrate, set boudrate = pclk/(baudrate_div *(1 + bts1_size + bts2_size)) */
  can_baudrate_struct.baudrate_div = 12;
  can_baudrate_struct.rsaw_size = CAN_RSAW_3TQ;
  can_baudrate_struct.bts1_size = CAN_BTS1_8TQ;
  can_baudrate_struct.bts2_size = CAN_BTS2_3TQ;
  if(can_baudrate_set(CAN1, &can_baudrate_struct) != SUCCESS)
  {
    return ERROR;
  }

  /* can filter 0 config */
  can_filter_init_struct.filter_activate_enable = TRUE;
  can_filter_init_struct.filter_mode = CAN_FILTER_MODE_ID_LIST;
  can_filter_init_struct.filter_fifo = CAN_FILTER_FIFO0;
  can_filter_init_struct.filter_number = 0;
  can_filter_init_struct.filter_bit = CAN_FILTER_32BIT;
  can_filter_init_struct.filter_id_high = (((FILTER_EXT_ID1 << 3) >> 16) & 0xFFFF);   /* extended identifier is 29 bit */
  can_filter_init_struct.filter_id_low = ((FILTER_EXT_ID1 << 3) & 0xFFFF) | 0x04;
  can_filter_init_struct.filter_mask_high = ((FILTER_EXT_ID2 << 3) >> 16) & 0xFFFF; /* extended identifier is 29 bit */
  can_filter_init_struct.filter_mask_low = ((FILTER_EXT_ID2 << 3) & 0xFFFF) | 0x04;
  can_filter_init(CAN1, &can_filter_init_struct);

  /* can filter 1 config */
  can_filter_init_struct.filter_activate_enable = TRUE;
  can_filter_init_struct.filter_mode = CAN_FILTER_MODE_ID_LIST;
  can_filter_init_struct.filter_fifo = CAN_FILTER_FIFO0;
  can_filter_init_struct.filter_number = 1;
  can_filter_init_struct.filter_bit = CAN_FILTER_32BIT;
  can_filter_init_struct.filter_id_high = FILTER_STD_ID1 << 5;  /* standard identifier is 11 bit */
  can_filter_init_struct.filter_id_low = 0;
  can_filter_init_struct.filter_mask_high = FILTER_STD_ID2 << 5;/* standard identifier is 11 bit */
  can_filter_init_struct.filter_mask_low = 0;
  can_filter_init(CAN1, &can_filter_init_struct);

  /* can interrupt config */
  nvic_irq_enable(CAN1_SE_IRQn, 0x00, 0x00);
  nvic_irq_enable(CAN1_RX0_IRQn, 0x00, 0x00);
  can_interrupt_enable(CAN1, CAN_RF0MIEN_INT, TRUE);
  can_interrupt_enable(CAN1, CAN_ETRIEN_INT, TRUE);
  can_interrupt_enable(CAN1, CAN_EOIEN_INT, TRUE);
  
  return SUCCESS;
}
  • DroneCAN初始化，其中canard可理解为句柄，memory_pool为缓存区，onTransferReceived为接收回调函数，shouldAcceptTransfer根据接收到的信息判断是否需要进入回调函数。
canardInit(&canard,
			memory_pool,
			sizeof(memory_pool),
			onTransferReceived,
			shouldAcceptTransfer,
			NULL);
  • 接收回调函数，在shouldAcceptTransfer函数中会对下发的DroneCAN信息根据传输类型进行初步筛选，然后对筛选过后的信息进入onTransferReceived函数解析处理。
static bool shouldAcceptTransfer(const CanardInstance *ins,
                                 uint64_t *out_data_type_signature,
                                 uint16_t data_type_id,
                                 CanardTransferType transfer_type,
                                 uint8_t source_node_id)
{
    if (transfer_type == CanardTransferTypeRequest) {
        // check if we want to handle a specific service request
        switch (data_type_id) {
        case UAVCAN_PROTOCOL_GETNODEINFO_ID: {
            *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE;
            return true;
        }
        case UAVCAN_PROTOCOL_PARAM_GETSET_ID: {
            *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE;
            return true;
        }
        case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: {
            *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE;
            return true;
        }
        case UAVCAN_PROTOCOL_RESTARTNODE_ID: {
            *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE;
            return true;
        }
	case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID:
	    *out_data_type_signature = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE;
	    return true;
	}
    }
    if (transfer_type == CanardTransferTypeResponse) {
        // check if we want to handle a specific service request
        switch (data_type_id) {
	case UAVCAN_PROTOCOL_FILE_READ_ID:
	    *out_data_type_signature = UAVCAN_PROTOCOL_FILE_READ_SIGNATURE;
	    return true;
	}
    }
    if (transfer_type == CanardTransferTypeBroadcast) {
        // see if we want to handle a specific broadcast packet
        switch (data_type_id) {
        case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID: {
            *out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE;
            return true;
        }
        case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID: {
            *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE;
            return true;
        }
        }
    }
    // we don't want any other messages
    return false;
}

static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
{
    // switch on data type ID to pass to the right handler function
    if (transfer->transfer_type == CanardTransferTypeRequest) {
        // check if we want to handle a specific service request
        switch (transfer->data_type_id) {
        case UAVCAN_PROTOCOL_GETNODEINFO_ID: {
            handle_GetNodeInfo(ins, transfer);
            break;
        }
        case UAVCAN_PROTOCOL_PARAM_GETSET_ID: {
            handle_param_GetSet(ins, transfer);
            break;
        }
        case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: {
            handle_param_ExecuteOpcode(ins, transfer);
            break;
        }
        case UAVCAN_PROTOCOL_RESTARTNODE_ID: {
            handle_RestartNode(ins, transfer);
            break;
        }
	case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID:
	    handle_begin_firmware_update(ins, transfer);
	    break;
	}
    }
    if (transfer->transfer_type == CanardTransferTypeResponse) {
	switch (transfer->data_type_id) {
	case UAVCAN_PROTOCOL_FILE_READ_ID:
	    handle_file_read_response(ins, transfer);
	    break;
        }
    }
    if (transfer->transfer_type == CanardTransferTypeBroadcast) {
        // check if we want to handle a specific broadcast message
        switch (transfer->data_type_id) {
        case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID: {
            handle_RawCommand(ins, transfer);
            break;
        }
        case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID: {
            handle_DNA_Allocation(ins, transfer);
            break;
        }
        }
    }
}
  • 接收解析示例。例如，飞控向CAN总线上发送DroneCAN协议的油门信号，则在shouldAcceptTransfer函数中会进入“if (transfer->transfer_type == CanardTransferTypeBroadcast)”如果传输类型为广播，“case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID”并且data_type_id为油门信号，“*out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE;”将输出信号置为油门信号，返回ture，接着进入到onTransferReceived函数，对应的回调函数是“handle_RawCommand(ins, transfer);”解码传输的油门信号，然后根据动力系统油门阈值做映射。
static void handle_RawCommand(CanardInstance *ins, CanardRxTransfer *transfer)
{
    struct uavcan_equipment_esc_RawCommand cmd;
    if (uavcan_equipment_esc_RawCommand_decode(transfer, &cmd)) {
        return;
    }
    // see if it is for us
    if (cmd.cmd.len <= settings.esc_index) {
        return;
    }
    // convert throttle to -1.0 to 1.0 range
    esc.throttle = cmd.cmd.data[(unsigned)settings.esc_index]/8192.0;
    esc.last_update_us = micros64();
}
  • 设置、获取本地节点。相当于设备ID，用于消息区分
void canardSetLocalNodeID(CanardInstance* ins, uint8_t self_node_id)
{
    CANARD_ASSERT(ins != NULL);

    if ((ins->node_id == CANARD_BROADCAST_NODE_ID) &&
        (self_node_id >= CANARD_MIN_NODE_ID) &&
        (self_node_id <= CANARD_MAX_NODE_ID))
    {
        ins->node_id = self_node_id;
    }
    else
    {
        CANARD_ASSERT(false);
    }
}

uint8_t canardGetLocalNodeID(const CanardInstance* ins)
{
    return ins->node_id;
}
  • 上传信号示例。以动力系统为例，需要向飞控上传电调信息、固件版本和节点心跳等。
static void send_ESCStatus(void)
{
    struct uavcan_equipment_esc_Status pkt;
    memset(&pkt, 0, sizeof(pkt));
    uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE];

    // make up some synthetic status data
    pkt.error_count = 0;
    pkt.voltage = 16.8 - 2.0 * esc.throttle;
    pkt.current = 20 * esc.throttle;
    pkt.temperature = C_TO_KELVIN(25.0);
    pkt.rpm = 10000 * esc.throttle;
    pkt.power_rating_pct = 100.0 * esc.throttle;

    uint32_t len = uavcan_equipment_esc_Status_encode(&pkt, buffer);
    
    static uint8_t transfer_id;
    
    canardBroadcast(&canard,
                    UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE,
                    UAVCAN_EQUIPMENT_ESC_STATUS_ID,
                    &transfer_id,
                    CANARD_TRANSFER_PRIORITY_LOW,
                    buffer,
                    len);
}

static void send_NodeStatus(void)
{
    uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE];

    node_status.uptime_sec = micros64() / 1000000ULL;
    node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
    node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
    node_status.sub_mode = 0;

    // put whatever you like in here for display in GUI
    node_status.vendor_specific_status_code = 1234;

    if (fwupdate.node_id != 0) {
	node_status.vendor_specific_status_code = fwupdate.offset / 1024;
	node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE;
    }

    uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer);

    static uint8_t transfer_id;

    canardBroadcast(&canard,
                    UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
                    UAVCAN_PROTOCOL_NODESTATUS_ID,
                    &transfer_id,
                    CANARD_TRANSFER_PRIORITY_LOW,
                    buffer,
                    len);
}

static void send_firmware_read(void)
{
    uint32_t now = millis32();
    if (now - fwupdate.last_read_ms < 750) {
        // the server may still be responding
        return;
    }
    fwupdate.last_read_ms = now;

    uint8_t buffer[UAVCAN_PROTOCOL_FILE_READ_REQUEST_MAX_SIZE];

    struct uavcan_protocol_file_ReadRequest pkt;
    memset(&pkt, 0, sizeof(pkt));

    pkt.path.path.len = strlen((const char *)fwupdate.path);
    pkt.offset = fwupdate.offset;
    memcpy(pkt.path.path.data, fwupdate.path, pkt.path.path.len);

    uint16_t total_size = uavcan_protocol_file_ReadRequest_encode(&pkt, buffer);

    canardRequestOrRespond(&canard,
			   fwupdate.node_id,
                           UAVCAN_PROTOCOL_FILE_READ_SIGNATURE,
                           UAVCAN_PROTOCOL_FILE_READ_ID,
			   &fwupdate.transfer_id,
                           CANARD_TRANSFER_PRIORITY_HIGH,
                           CanardRequest,
                           &buffer[0],
                           total_size);
}
  • 实际上传实现。如上面上传的数据都会进入canardBroadcast函数，而在该函数里面最终是将数据依次写入队列，通过不断判断队列是否为空去调用底层普通CAN的发送接口去向CAN总线广播
for ( (txf = canardPeekTxQueue(&canard)) != NULL;) 
{
    const int16_t tx_res = socketcanTransmit(socketcan, txf, 0);
    if (tx_res < 0) {         // Failure - drop the frame
        canardPopTxQueue(&canard);
    }
    else if (tx_res > 0)    // Success - just drop the frame
    {
        canardPopTxQueue(&canard);
    }
    else                    // Timeout - just exit and try again later
    {
        break;
    }
}
  • 动态节点ID分配，当节点ID未被人为分配时，会以0的形式去上传数据，则当地面站接收到节点值为0的设备会自动给其分配一个节点ID
if (canardGetLocalNodeID(&canard) == 0) 
{
    // we're still waiting for a DNA allocation of our node ID
    if (millis32() > DNA.send_next_node_id_allocation_request_at_ms) {
        request_DNA();
    }
    continue;
}

static void request_DNA()
{
    const uint32_t now = millis32();
    static uint8_t node_id_allocation_transfer_id = 0;

    DNA.send_next_node_id_allocation_request_at_ms =
        now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
        (random() % UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);

    // Structure of the request is documented in the DSDL definition
    // 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 (DNA.node_id_allocation_unique_id_offset == 0) {
        allocation_request[0] |= 1;     // First part of unique ID
    }

    uint8_t my_unique_id[16];
    getUniqueID(my_unique_id);

    static const uint8_t MaxLenOfUniqueIDInRequest = 6;
    uint8_t uid_size = (uint8_t)(16 - DNA.node_id_allocation_unique_id_offset);
    
    if (uid_size > MaxLenOfUniqueIDInRequest) {
        uid_size = MaxLenOfUniqueIDInRequest;
    }

    memmove(&allocation_request[1], &my_unique_id[DNA.node_id_allocation_unique_id_offset], uid_size);

    // Broadcasting the request
    const int16_t bcast_res = canardBroadcast(&canard,
                                              UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE,
                                              UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
                                              &node_id_allocation_transfer_id,
                                              CANARD_TRANSFER_PRIORITY_LOW,
                                              &allocation_request[0],
                                              (uint16_t) (uid_size + 1));
    if (bcast_res < 0) {
        printf("Could not broadcast ID allocation req; error %d\n", bcast_res);
    }

    // Preparing for timeout; if response is received, this value will be updated from the callback.
    DNA.node_id_allocation_unique_id_offset = 0;
}

可参考如下形式进行设计：

int main(int argc, char** argv)
{
    if (argc < 2) {
        (void)fprintf(stderr,
                      "Usage:\n"
                      "\t%s <can iface name>\n",
                      argv[0]);
        return 1;
    }

    load_settings();

    /*
     * Initializing the CAN backend driver
     */
    LinuxCANInstance can;
    const char* const can_iface_name = argv[1];
    int16_t res = LinuxCANInit(&can, can_iface_name);
    if (res < 0) {
        (void)fprintf(stderr, "Failed to open CAN iface '%s'\n", can_iface_name);
        return 1;
    }

    /*
     Initializing the Libcanard instance.
     */
    canardInit(&canard,
               memory_pool,
               sizeof(memory_pool),
               onTransferReceived,
               shouldAcceptTransfer,
               NULL);

    if (settings.can_node > 0) {
        canardSetLocalNodeID(&canard, settings.can_node);
    } else {
        printf("Waiting for DNA node allocation\n");
    }

    /*
      Run the main loop.
     */
    uint64_t next_1hz_service_at = micros64();
    uint64_t next_50hz_service_at = micros64();

    while (true) {
        processTxRxOnce(&can, 10);

        const uint64_t ts = micros64();

        if (canardGetLocalNodeID(&canard) == CANARD_BROADCAST_NODE_ID) {
            // waiting for DNA
        }

        // see if we are still doing DNA
        if (canardGetLocalNodeID(&canard) == CANARD_BROADCAST_NODE_ID) {
            // we're still waiting for a DNA allocation of our node ID
            if (millis32() > DNA.send_next_node_id_allocation_request_at_ms) {
                request_DNA();
            }
            continue;
        }

        if (ts >= next_1hz_service_at) {
            next_1hz_service_at += 1000000ULL;
            process1HzTasks(ts);
        }
        if (ts >= next_50hz_service_at) {
            next_50hz_service_at += 1000000ULL/50U;
            send_ESCStatus();
	}

	if (fwupdate.node_id != 0) {
	    send_firmware_read();
	}
    }

    return 0;
}

http://www.kler.cn/a/456492.html

相关文章：

  • streamlit、shiny、gradio、fastapi四个web APP平台体验
  • 【SQL Server】教材数据库(1)
  • 2、Bert论文笔记
  • 【每日学点鸿蒙知识】箭头函数、Watch状态变量、H5获取定位数据、前后台切换、长按事件
  • HTML——29. 音频引入二
  • 手机发烫怎么解决？
  • 单例模式懒汉式、饿汉式（线程安全）
  • Live555、FFmpeg、GStreamer介绍
  • acitvemq AMQP:因为消息映射策略配置导致的MQTT接收JMS消息乱码问题 x-opt-jms-dest x-opt-jms-msg-type
  • 机器学习基本概念，基本步骤，分类，简单理解，线性模型
  • 【期末复习】JavaEE（下）
  • Arduino中借助LU-ASR01实现语音识别
  • Go语言基础语法
  • RNA-Seq 数据集、比对和标准化
  • 基于GA遗传优化TCN时间卷积神经网络时间序列预测算法matlab仿真
  • 【AIGC-ChatGPT副业提示词指令 - 动图】魔法咖啡馆：一个融合创意与治愈的互动体验设计
  • 总结一下本次使用docker部署遇到的问题
  • 【已解决】图片png转ico格式
  • 伏羲0.13（文生图）
  • 三极管、运放和稳压二极管恒流电路设计原理分析
  • Vue中接入萤石等直播视频（更新中ing）
  • 如何在Express.js中定义多个HTTP方法？
  • ＜packaging＞jar＜/packaging＞和＜packaging＞pom＜/packaging＞的区别
  • nginx Rewrite 相关功能
  • fopen的概念和使用方法
  • 正则表达式 - 使用总结