ArduPilot 第11章 AP_Proximity

axzg

已于 2024-05-21 16:18:19 修改

阅读量892

点赞数 22

文章标签：算法

于 2024-03-09 22:24:55 首次发布

本文链接：https://blog.csdn.net/zhanggeng0101/article/details/136576487

版权

文章目录

前言
一、AP_Proximity构造函数赋值
二·、AP_Proximity初始化
三、AP_Proximity后端
四、AP_Proximity前端
总结

参考文献
ardupilot开发 —传感器驱动，外设驱动，接近传感器篇
https://blog.csdn.net/weixin_43321489/article/details/133312281

ArduPilot开源代码之AP_Param
http://www.360doc.com/content/23/1217/10/1676247_1107842455.shtml
3.54 load defaults file

前言

本文就介绍AP_Proximity的基础内容。

一、AP_Proximity构造函数赋值

1 AP_Proximity例化
\ArduCopter\Parameters.h
AP_Proximity类的构造函数比初始化AP_Proximity::init()先执行

class ParametersG2 {
	public:
		...
		#if HAL_PROXIMITY_ENABLED
    		// proximity (aka object avoidance) library
    		AP_Proximity proximity;
		#endif
		...
}

2 AP_Proximity构造函数
\libraries\AP_Proximity\AP_Proximity.cpp
其中this是类当前的实例
var_info的类型为AP_Param::GroupInfo

AP_Proximity::AP_Proximity()
{
    AP_Param::setup_object_defaults(this, var_info);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    if (_singleton != nullptr) {
        AP_HAL::panic("AP_Proximity must be singleton");
    }
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
    _singleton = this;
}

3 AP_Param::setup_object_defaults函数
在group中变量加载默认值。这是一个静态函数，应该在对象构造函数中调用。
\libraries\AP_Param\AP_Param.cpp

void AP_Param::setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info)
{
    ptrdiff_t base = (ptrdiff_t)object_pointer;
    uint8_t type;
    for (uint8_t i=0;
         (type=group_info[i].type) != AP_PARAM_NONE;
         i++) {
        if (type <= AP_PARAM_FLOAT) {
            void *ptr = (void *)(base + group_info[i].offset);
            set_value((enum ap_var_type)type, ptr,
                      get_default_value((const AP_Param *)ptr, &group_info[i].def_value));
        }
    }
}

4 AP_Param::set_value函数
设置AP_Param变量为指定值
\libraries\AP_Param\AP_Param.cpp

void AP_Param::set_value(enum ap_var_type type, void *ptr, float value)
{
    switch (type) {
    case AP_PARAM_INT8:
        ((AP_Int8 *)ptr)->set(value);
        break;
    case AP_PARAM_INT16:
        ((AP_Int16 *)ptr)->set(value);
        break;
    case AP_PARAM_INT32:
        ((AP_Int32 *)ptr)->set(value);
        break;
    case AP_PARAM_FLOAT:
        ((AP_Float *)ptr)->set(value);
        break;
    default:
        break;
    }
}

5 AP_Param::get_default_value
给定一个指向flash中默认值的指针，找到一个默认值
\libraries\AP_Param\AP_Param.cpp

/* 
   find a default value given a pointer to a default value in flash
 */
float AP_Param::get_default_value(const AP_Param *vp, const float *def_value_ptr)
{
    for (uint16_t i=0; i<num_param_overrides; i++) {
        if (vp == param_overrides[i].object_ptr) {
            return param_overrides[i].value;
        }
    }
    return *def_value_ptr;
}

6 static const struct AP_Param::GroupInfo var_info[]
\libraries\AP_Proximity\AP_Proximity.h

class AP_Proximity
{
public
	...
	// parameter list
    static const struct AP_Param::GroupInfo var_info[];
	...
}

7 AP_Param::GroupInfo AP_Proximity::var_info[]
用户可设置参数表
数组 var_info[ ] 赋值语句
\libraries\AP_Proximity\AP_Proximity.cpp

const AP_Param::GroupInfo AP_Proximity::var_info[] = {
   
    AP_GROUPINFO_FLAGS("_TYPE",   1, AP_Proximity, _type[0], 0, AP_PARAM_FLAG_ENABLE),  
    AP_GROUPINFO("_ORIENT", 2, AP_Proximity, _orientation[0], 0),
    AP_GROUPINFO("_YAW_CORR", 3, AP_Proximity, _yaw_correction[0], 0),
    AP_GROUPINFO("_IGN_ANG1", 4, AP_Proximity, _ignore_angle_deg[0], 0),
    AP_GROUPINFO("_IGN_WID1", 5, AP_Proximity, _ignore_width_deg[0], 0),
    AP_GROUPINFO("_IGN_ANG2", 6, AP_Proximity, _ignore_angle_deg[1], 0),
    AP_GROUPINFO("_IGN_WID2", 7, AP_Proximity, _ignore_width_deg[1], 0),
    AP_GROUPINFO("_IGN_ANG3", 8, AP_Proximity, _ignore_angle_deg[2], 0),
    AP_GROUPINFO("_IGN_WID3", 9, AP_Proximity, _ignore_width_deg[2], 0),
    AP_GROUPINFO("_IGN_ANG4", 10, AP_Proximity, _ignore_angle_deg[3], 0),
    AP_GROUPINFO("_IGN_WID4", 11, AP_Proximity, _ignore_width_deg[3], 0),
    AP_GROUPINFO("_IGN_ANG5", 12, AP_Proximity, _ignore_angle_deg[4], 0),
    AP_GROUPINFO("_IGN_WID5", 13, AP_Proximity, _ignore_width_deg[4], 0),
    AP_GROUPINFO("_IGN_ANG6", 14, AP_Proximity, _ignore_angle_deg[5], 0),
    AP_GROUPINFO("_IGN_WID6", 15, AP_Proximity, _ignore_width_deg[5], 0),
    AP_GROUPINFO_FRAME("_IGN_GND", 16, AP_Proximity, _ign_gnd_enable, 0, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER),
    AP_GROUPINFO("_LOG_RAW", 17, AP_Proximity, _raw_log_enable, 0),
    AP_GROUPINFO("_FILT", 18, AP_Proximity, _filt_freq, 0.25f),
    AP_GROUPINFO("_MIN", 19, AP_Proximity, _min_m, 0.0f),
    AP_GROUPINFO("_MAX", 20, AP_Proximity, _max_m, 0.0f),
    AP_GROUPEND
};

var_info[] 数组的每个元素（每一行宏定义）代表每个独立参数的信息合集合；可知一共有三种不同的宏定义可以使用，下面分别解析；

以下面例子为例

AP_GROUPINFO_FRAME("_IGN_GND", 16, AP_Proximity, _ign_gnd_enable, 0, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER),

请添加图片描述
(1)
var_info[] 数组每个元素分别保存了类AP_Proximity中的_filt_freq、_ign_gnd_enable等这几个成员的相关信息（包括变量名称，默认值，地址偏移量，数据类型等信息），以便传递给构造函数根据地面站的用户设置进行赋值；
比如，用户在地面站上设定参数AP_Proximity_IGN_GND为0，则代码将会把PRX_IGN_GND的值通过名称匹配地址匹配的方式赋值给当前AP_Proximity实例中的成员_ign_gnd_enable。
(2)
每个用户参数的初始化信息都在对应的类的var_info[]中，然后类、子类调用自己的构造函数，构造函数再调用AP_Param::setup_object_defaults(this, var_info);来对参数进行初始化.。
(3)
地面站上的用户参数一般都可以找到与之相对应的类参数，一般都被定义在对应名称的类中，例如地面站参数名为PRX_IGN_GND，与它对应的类参数在类AP_Proximity中名为_ign_gnd_enable，PRX_IGN_GND的值会被传递给_ign_gnd_enable。
(4)
关于var_info[]的一点补充，var_info[]的每个元素即某个参数的初始化信息，如果参数是普通类型则默认值是union中的def_value，如果类型是类则默认值是union中的group_info。

8 param_overrides补充知识
8.1 AP_Param::load_all()
从EEPROM载入所有变量
\libraries\AP_Param\AP_Param.cpp

bool AP_Param::load_all()
{
	...
	reload_defaults_file(false);
	...
}

8.2 void Copter::allocate_motors(void)
\ArduCopter\system.cpp

void Copter::allocate_motors(void)
{
	...
	AP_Param::load_object_from_eeprom(circle_nav, circle_nav->var_info);
	...
	 AP_Param::reload_defaults_file(true);
	...
}

8.3 void AP_Param::reload_defaults_file
从ha.util默认文件或嵌入参数区域重新装载
\libraries\AP_Param\AP_Param.cpp

void AP_Param::reload_defaults_file(bool last_pass)
{
#if AP_PARAM_MAX_EMBEDDED_PARAM > 0
    if (param_defaults_data.length != 0) {
        load_embedded_param_defaults(last_pass);
        return;
    }
#endif

#if HAL_OS_POSIX_IO == 1
    const char *default_file = hal.util->get_custom_defaults_file();
    if (default_file) {
        if (load_defaults_file(default_file, last_pass)) {
            printf("Loaded defaults from %s\n", default_file);
        } else {
            AP_HAL::panic("Failed to load defaults from %s\n", default_file);
        }
    }
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
    hal.util->set_cmdline_parameters();
#endif
}

8.4 void AP_Param::load_embedded_param_defaults函数
从嵌入参数区域加载默认参数集
\libraries\AP_Param\AP_Param.cpp

void AP_Param::load_embedded_param_defaults(bool last_pass)
{
	...
	param_overrides = new param_override[num_defaults];
	...
	param_overrides[idx].object_ptr = vp;
    param_overrides[idx].value = value;
    param_overrides[idx].read_only = read_only;
    ...   
    num_param_overrides = num_defaults;
}

8.5 AP_Param::load_defaults_file函数
从文件中获取默认变量集
\libraries\AP_Param\AP_Param.cpp

bool AP_Param::load_defaults_file(const char *filename, bool last_pass)
{
    ...
    delete[] param_overrides;
    num_param_overrides = 0;
    num_read_only = 0;

    param_overrides = new param_override[num_defaults];
	...
	if (!read_param_defaults_file(pname, last_pass)) {
           free(mutable_filename);
           return false;
    
   }
   ...
   num_param_overrides = num_defaults;
   ...
}

二·、AP_Proximity初始化

1 Copter::init_ardupilot函数

\ardupilot\ArduCopter\system.cpp

void Copter::init_ardupilot()
{
	...
	#if HAL_PROXIMITY_ENABLED
	    // init proximity sensor
	    g2.proximity.init();
	
	#endif
	...
}

2 AP_Proximity::init函数

初始化proximity类。我们在这里检测连接的传感器
\libraries\AP_Proximity\AP_Proximity.cpp

// initialise the Proximity class. We do detection of attached sensors here
// we don't allow for hot-plugging of sensors (i.e. reboot required)
void AP_Proximity::init(void)
{
    if (num_instances != 0) {
        // init called a 2nd time?
        return;
    }
    for (uint8_t i=0; i<PROXIMITY_MAX_INSTANCES; i++) {
        detect_instance(i);
        if (drivers[i] != nullptr) {
            // we loaded a driver for this instance, so it must be
            // present (although it may not be healthy)
            num_instances = i+1;
        }

        // initialise status
        state[i].status = Status::NotConnected;
    }
}

3 PROXIMITY_MAX_INSTANCES

\libraries\AP_Proximity\AP_Proximity.h

#define PROXIMITY_MAX_INSTANCES             1   
#define PROXIMITY_MAX_IGNORE                6   
#define PROXIMITY_MAX_DIRECTION 8
#define PROXIMITY_SENSOR_ID_START 10

三、AP_Proximity后端

1 detect_instance函数
检测是否连接了proximity传感器
\libraries\AP_Proximity\AP_Proximity.cpp中

//  detect if an instance of a proximity sensor is connected.
void AP_Proximity::detect_instance(uint8_t instance)
{
    switch (get_type(instance)) {
    case Type::None:
        return;
    case Type::RPLidarA2:
        if (AP_Proximity_RPLidarA2::detect()) {
            state[instance].instance = instance;
            drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance]);
            return;
        }
        break;
    ...
    case Type::RangeFinder:
        state[instance].instance = instance;
        drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance]);
        return;
    ...
    
    }
}

2 get_type函数
libraries\AP_Proximity\AP_Proximity.cpp中

AP_Proximity::Type AP_Proximity::get_type(uint8_t instance) const
{
    if (instance < PROXIMITY_MAX_INSTANCES) {
        return (Type)((uint8_t)_type[instance]);
    }
    return Type::None;
}

3 AP_Proximity_RPLidarA2::detect函数
以case Type::RPLidarA2为例
AP_Proximity_RPLidarA2::detect(serial_instance)是如何检测雷达是否正常接入的？
类AP_Proximity_RPLidarA2中找死都找不到detect函数。
detect函数在AP_Proximity_RPLidarA2的父类AP_Proximity_Backend_Serial中

\libraries\AP_Proximity\AP_Proximity_RPLidarA2.h

class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend_Serial
{
	...
}

通过查找已配置的串行端口来检测是否连接了接近传感器
\libraries\AP_Proximity\AP_Proximity_Backend_Serial.cpp

bool AP_Proximity_Backend_Serial::detect()
{
    return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_Lidar360, 0);
}

4 AP_SerialManager::have_serial函数

如果我们配置了给定的串行协议，则返回true
\libraries\AP_SerialManager\AP_SerialManager.cpp

// have_serial - return true if we have the given serial protocol configured
bool AP_SerialManager::have_serial(enum SerialProtocol protocol, uint8_t instance) const
{
    return find_protocol_instance(protocol, instance) != nullptr;
}

5 AP_SerialManager::find_protocol_instance
查找匹配的协议
\libraries\AP_SerialManager\AP_SerialManager.cpp

const AP_SerialManager::UARTState *AP_SerialManager::find_protocol_instance(enum SerialProtocol protocol, uint8_t instance) const
{
    uint8_t found_instance = 0;

    // search for matching protocol
    for(uint8_t i=0; i<SERIALMANAGER_NUM_PORTS; i++) {
        if (protocol_match(protocol, (enum SerialProtocol)state[i].protocol.get())) {
            if (found_instance == instance) {
                return &state[i];
            }
            found_instance++;
        }
    }

    // if we got this far we did not find the uart
    return nullptr;
}

6 AP_SerialManager::protocol_match
-如果协议匹配则返回true
\libraries\AP_SerialManager\AP_SerialManager.cpp

bool AP_SerialManager::protocol_match(enum SerialProtocol protocol1, enum SerialProtocol protocol2) const
{
    // check for obvious match
    if (protocol1 == protocol2) {
        return true;
    }

    // mavlink match
    if (((protocol1 == SerialProtocol_MAVLink) || (protocol1 == SerialProtocol_MAVLink2)) &&
        ((protocol2 == SerialProtocol_MAVLink) || (protocol2 == SerialProtocol_MAVLink2))) {
        return true;
    }

    // gps match
    if (((protocol1 == SerialProtocol_GPS) || (protocol1 == SerialProtocol_GPS2)) &&
        ((protocol2 == SerialProtocol_GPS) || (protocol2 == SerialProtocol_GPS2))) {
        return true;
    }

    return false;
}

7 AP_Proximity_RPLidarA2构造函数调用
查看以下代码

void AP_Proximity::detect_instance(uint8_t instance)
{
    switch (get_type(instance)) {
    case Type::None:
        return;
    case Type::RPLidarA2:
        if (AP_Proximity_RPLidarA2::detect()) {
            state[instance].instance = instance;
            drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance]);
            return;
        }
        break;

由于在AP_Proximity::detect_instance函数中
先调用
AP_Proximity_RPLidarA2::detect()返回为1时候
再调用
drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance])
此时调用了AP_Proximity_RPLidarA2的构造函数
构造函数还初始化接近传感器。请注意，在detect（）返回true之前，不会调用此构造函数。
\libraries\AP_Proximity\AP_Proximity_Backend_Serial.cpp

AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial(AP_Proximity &_frontend,
                                                         AP_Proximity::Proximity_State &_state) :
    AP_Proximity_Backend(_frontend, _state)
{
    const AP_SerialManager &serial_manager = AP::serialmanager();
    _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0);
    if (_uart != nullptr) {
        // start uart with larger receive buffer
        _uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, 0), rxspace(), 0);
    }
}

(1)serialmanager
单例实例化AP_SerialManager
\AP_SerialManager\AP_SerialManager.h

namespace AP {
AP_SerialManager &serialmanager()
{
    return *AP_SerialManager::get_singleton();
}
}

(2)find_serial
// find_serial -为允许给定协议的第一个实例搜索可用的串行端口
//如果搜索第一个实例，则instance应为0，第二个实例为1，以此类推
//成功时返回uart，如果找不到串口则返回nullptr
\libraries\AP_SerialManager\AP_SerialManager.cpp

AP_HAL::UARTDriver *AP_SerialManager::find_serial(enum SerialProtocol protocol, uint8_t instance) const
{
    const struct UARTState *_state = find_protocol_instance(protocol, instance);
    if (_state == nullptr) {
        return nullptr;
    }
    const uint8_t serial_idx = _state - &state[0];

    // set options before any user does begin()
    AP_HAL::UARTDriver *port = hal.serial(serial_idx);
    if (port) {
        port->set_options(_state->options);
    }
    return port;
}

(3)_HAL::HAL::serial函数

使用SERIALn编号访问串口
\libraries\AP_HAL\HAL.cpp

// access serial ports using SERIALn numbering
AP_HAL::UARTDriver* AP_HAL::HAL::serial(uint8_t sernum) const
{
    UARTDriver **uart_array = const_cast<UARTDriver**>(&uartA);
    // this mapping captures the historical use of uartB as SERIAL3
    const uint8_t mapping[] = { 0, 2, 3, 1, 4, 5, 6, 7, 8, 9 };
    static_assert(sizeof(mapping) == num_serial, "num_serial must match mapping");
    if (sernum >= num_serial) {
        return nullptr;
    }
    return uart_array[mapping[sernum]];
}

(4)AP_HAL::UARTDriver* uartA;
\libraries\AP_HAL\HAL.h

class AP_HAL::HAL {
public:
    HAL(AP_HAL::UARTDriver* _uartA, // console
        AP_HAL::UARTDriver* _uartB, // 1st GPS
        AP_HAL::UARTDriver* _uartC, // telem1
        AP_HAL::UARTDriver* _uartD, // telem2
        AP_HAL::UARTDriver* _uartE, // 2nd GPS
        AP_HAL::UARTDriver* _uartF, // extra1
        AP_HAL::UARTDriver* _uartG, // extra2
        AP_HAL::UARTDriver* _uartH, // extra3
        AP_HAL::UARTDriver* _uartI, // extra4
        AP_HAL::UARTDriver* _uartJ, // extra5
        ...
       
		:
        uartA(_uartA),
        uartB(_uartB),
        uartC(_uartC),
        uartD(_uartD),
        uartE(_uartE),
        uartF(_uartF),
        uartG(_uartG),
        uartH(_uartH),
        uartI(_uartI),
        uartJ(_uartJ),
        ...
private:
    // the uartX ports must be contiguous in ram for the serial() method to work
    AP_HAL::UARTDriver* uartA;
    AP_HAL::UARTDriver* uartB;
    AP_HAL::UARTDriver* uartC;
    AP_HAL::UARTDriver* uartD;
    AP_HAL::UARTDriver* uartE;
    AP_HAL::UARTDriver* uartF;
    AP_HAL::UARTDriver* uartG;
    AP_HAL::UARTDriver* uartH;
    AP_HAL::UARTDriver* uartI;
    AP_HAL::UARTDriver* uartJ;

}

(5)AP_SerialManager::find_baudrate
// find_baudrate-搜索第一个允许给定协议的可用串行端口
//如果搜索第一个实例，则instance应为0，第二个实例为1，以此类推
//成功时返回波特率，如果找不到串口则返回0
\libraries\AP_SerialManager

uint32_t AP_SerialManager::find_baudrate(enum SerialProtocol protocol, uint8_t instance) const
{
    const struct UARTState *_state = find_protocol_instance(protocol, instance);
    if (_state == nullptr) {
        return 0;
    }
    return map_baudrate(_state->baud);
}

(6)get_mavlink_protocol
// get_mavlink_protocol -提供特定的MAVLink协议
//指定通道，如果没有找到，则SerialProtocol_None

\libraries\AP_SerialManager\AP_SerialManager.cpp

AP_SerialManager::SerialProtocol AP_SerialManager::get_mavlink_protocol(mavlink_channel_t mav_chan) const
{
    uint8_t instance = 0;
    uint8_t chan_idx = (uint8_t)(mav_chan - MAVLINK_COMM_0);
    for (uint8_t i=0; i<SERIALMANAGER_NUM_PORTS; i++) {
        if (state[i].protocol == SerialProtocol_MAVLink ||
            state[i].protocol == SerialProtocol_MAVLink2) {
            if (instance == chan_idx) {
                return (SerialProtocol)state[i].protocol.get();
            }
            instance++;
        }
    }
    return SerialProtocol_None;
}

四、AP_Proximity前端

1 SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 50, 36),
\ArduCopter\Copter.cpp

const AP_Scheduler::Task Copter::scheduler_tasks[] = {
	...
    SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 50,  36),
    ...
}

2 AP_Proximity::update函数

更新所有实例的接近状态。这应该由主循环以较高的速率调用
\libraries\AP_SerialManager\AP_SerialManager.cpp

void AP_Proximity::update(void)
{
    for (uint8_t i=0; i<num_instances; i++) {
        if (!valid_instance(i)) {
            continue;
        }
        drivers[i]->update();
        drivers[i]->boundary_3D_checks();
    }

    // work out primary instance - first sensor returning good data
    for (int8_t i=num_instances-1; i>=0; i--) {
        if (drivers[i] != nullptr && (state[i].status == Status::Good)) {
            primary_instance = i;
        }
    }
}

3 void AP_Proximity_RPLidarA2::update(void)
更新传感器的_rp_state
\libraries\AP_Proximity\AP_Proximity_RPLidarA2.cpp

void AP_Proximity_RPLidarA2::update(void)
{
	...
		// if LIDAR in known state
    if (_initialised) {
        get_readings();
    }
	...
}

4 AP_Proximity_RPLidarA2::get_readings函数
\libraries\AP_Proximity\AP_Proximity_RPLidarA2.cpp

void AP_Proximity_RPLidarA2::get_readings()
{
			...
			case rp_measurements:
            ...    
              parse_response_data();
            ...     
            break;

5 AP_Proximity_RPLidarA2::parse_response_data函数
\libraries\AP_Proximity\AP_Proximity_RPLidarA2.cpp

void AP_Proximity_RPLidarA2::parse_response_data()
{
	switch (_response_type){
        case ResponseType_SCAN:
        	...
        	// update OA database
            database_push(_last_angle_deg, _last_distance_m);
            ...
        break;
            
}

6 AP_Proximity_Backend::database_push函数
用地球坐标系点更新目标避障数据库
\libraries\AP_Proximity\AP_Proximity_RPLidarA2.cpp

void AP_Proximity_Backend::database_push(float angle, float distance)
{
    Vector3f current_pos;
    Matrix3f body_to_ned;

    if (database_prepare_for_push(current_pos, body_to_ned)) {
        database_push(angle, distance, AP_HAL::millis(), current_pos, body_to_ned);
    }
}