APM-3.5.2 UAVCAN笔记

20180402：
UAVCAN使用的主要逻辑代码，在AP_UAVCAN.cpp文件中，AP_UAVCAN类的任务是在硬件配置支持的情况下使用UAVCAN协议，进行数据的收发。UAVCAN协议的收发逻辑代码太过于复杂，看了几天仍然云里雾里，所以我决定转换思路，不再过深的去了解协议的收发逻辑代码，转而去了解需要经过哪些步骤，使用哪些UAVCAN协议提供的API，可以完成收发任务。
接下来以GPS数据为例，来梳理使用UAVCAN协议完成数据收发的过程：
飞控要通过UAVCAN使用GPS信息，需要添加使用UAVCAN协议中GPS数据的类，APM中此类在AP_GPS_UAVCAN.cpp和AP_GPS_UAVCAN.h中。AP_UAVCAN类读到UAVCAN协议的GPS信息后，存储在_gps_node_state[AP_UAVCAN_MAX_GPS_NODES]结构体数组中，通过update_gps_state函数，将GPS信息传递给AP_GPS_UAVCAN类。
AP_UAVCAN构造函数：
_gps_nodes[AP_UAVCAN_MAX_GPS_NODES]：存放着发送GPS信息的节点（下文简称为GPS信息源）ID
_gps_node_taken[AP_UAVCAN_MAX_GPS_NODES]
_gps_listener_to_node[AP_UAVCAN_MAX_LISTENERS]
_gps_listeners[AP_UAVCAN_MAX_LISTENERS]
注：宏定义见附录。

AP_UAVCAN::AP_UAVCAN() :
    _node_allocator(
        UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_SIZE)
{
    AP_Param::setup_object_defaults(this, var_info);


    for (uint8_t i = 0; i < UAVCAN_RCO_NUMBER; i++) {
        _rco_conf[i].active = false;
    }


    for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) {
        _gps_nodes[i] = UINT8_MAX;
        _gps_node_taken[i] = 0;
    }


    for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) {
        _baro_nodes[i] = UINT8_MAX;
        _baro_node_taken[i] = 0;
    }


    for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) {
        _mag_nodes[i] = UINT8_MAX;
        _mag_node_taken[i] = 0;
    }


    for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) {
        _gps_listener_to_node[i] = UINT8_MAX;
        _gps_listeners[i] = nullptr;


        _baro_listener_to_node[i] = UINT8_MAX;
        _baro_listeners[i] = nullptr;


        _mag_listener_to_node[i] = UINT8_MAX;
        _mag_listeners[i] = nullptr;
    }


    _rc_out_sem = hal.util->new_semaphore();


    debug_uavcan(2, "AP_UAVCAN constructed\n\r");
}

这些变量在构造函数中赋值为空或空闲，_gps_nodes[]在AP_UAVCAN.cpp的gnss_fix_cb函数中被调用的find_gps_node函数赋值。

AP_GPS::GPS_State *AP_UAVCAN::find_gps_node(uint8_t node)
{
    // Check if such node is already defined
    for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) {
        if (_gps_nodes[i] == node) {
            return &_gps_node_state[i];
        }
    }


    // If not - try to find free space for it
    for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) {
        if (_gps_nodes[i] == UINT8_MAX) {
            _gps_nodes[i] = node;
            return &_gps_node_state[i];
        }
    }


    // If no space is left - return nullptr
    return nullptr;
}

find_gps_node函数完成两个功能，一、如果_gps_node[]中有存放GPS信息源的节点ID，则返回，与节点ID对应的_gps_node_state[]的指针。二、如果_gps_node中并没有存放GPS信息源的节点ID，则遍历_gps_node[]中处于空闲的成员，将GPS信息源的节点ID赋值给找到的第一个空闲的_gps_node[]。并返回与之对应的_gps_node_state[]指针。_gps_node[]与_gps_node_state[]是一一对应的。但_gps_node[]存放着哪个GPS信息源是不固定的，是程序运行时确定的，相应的_gps_node_state[]存放这那个GPS信息源的GPS数据也是不固定的，是程序运行时确定的。

_gps_node_taken[]、_gps_listener_to_node[]、_gps_listeners[]这些变量在AP_GPS.cpp的detect_instance函数中被调用的AP_UAVCAN中的函数赋有含义的值。

#if HAL_WITH_UAVCAN
    // user has to explicitly set the UAVCAN type, do not use AUTO
    case GPS_TYPE_UAVCAN:
        dstate->auto_detected_baud = false; // specified, not detected
        if (AP_BoardConfig_CAN::get_can_num_ifaces() >= 1) {
            for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
                if (hal.can_mgr[i] != nullptr) {
                    AP_UAVCAN *uavcan = hal.can_mgr[i]->get_UAVCAN();


                    if (uavcan != nullptr) {
                        uint8_t gps_node = uavcan->find_gps_without_listener();


                        if (gps_node != UINT8_MAX) {
                            new_gps = new AP_GPS_UAVCAN(*this, state[instance], nullptr);
                            ((AP_GPS_UAVCAN*) new_gps)->set_uavcan_manager(i);
                            if (uavcan->register_gps_listener_to_node(new_gps, gps_node)) {
                                if (AP_BoardConfig_CAN::get_can_debug() >= 2) {
                                    printf("AP_GPS_UAVCAN registered\n\r");
                                }
                                goto found_gps;
                            } else {
                                delete new_gps;
                            }
                        }
                    }
                }
            }
        }
        return;
#endif

附录

#define AP_UAVCAN_MAX_GPS_NODES 4
#define AP_UAVCAN_MAX_LISTENERS 4
AP_BoardConfig_CAN::get_can_num_ifaces()，返回启用的CAN接口总数。

猜你喜欢