ArduPilot第4章rangefinder

axzg

已于 2024-05-21 16:20:05 修改

阅读量1.1k

点赞数 20

文章标签：算法

于 2024-02-20 17:16:59 首次发布

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

版权

文章目录

前言
一、传感器代码结构
二、传感器初始化
三、后端调用
四、前端调用
- 1.前端
总结

参考文献
https://blog.csdn.net/weixin_43321489/article/details/133312281
https://blog.csdn.net/qq_20016593/article/details/132715941

The Cube Orange 标准套装
https://muginuav.en.made-in-china.com/product/cwmQPxIEIrWb/China-Pixhawk-Autopilot-The-Cube-Orange-Standard-Set.html

Ardupilot设备驱动 IIC、SPI、USART
https://www.cnblogs.com/BlogsOfLei/p/7745636.html

APM代码阅读（一）：串口驱动
https://blog.51cto.com/u_15352549/6507034

ArduPilot之开源代码Sensor Drivers设计
https://blog.csdn.net/lida2003/article/details/130438349

前言

以测距传感器为例，介绍ArduPilot传感器开发流程

一、传感器代码结构

ardupilot中传感器驱动的重要结构是前台和后台分离。
第1步 Library库调用前端程序
第2步前端程序调用后端程序
第3步后端程序一般是个for循环

二、传感器初始化

以测距传感器为例，讲解ardupilot是如何初始化传感器驱动

1.void AP_Vehicle::setup()*

此函数仅在启动时调用一次。用于初始化一些必要的任务。此函数由 HAL 中的 main() 函数调用。
\libraries\AP_Vehicle\AP_Vehicle.cpp

void AP_Vehicle::setup()
{
	init_ardupilot();
}

2.AP_Vehicle& vehicle = copter

\ArduCopter\ArduCopter.cpp

Copter copter;
AP_Vehicle& vehicle = copter;

3.void Copter::init_ardupilot()

init_ardupilot() 函数将处理空中重启所需的一切。稍后将确定飞行器是否真的在地面上，并在这种情况下处理地面启动。
\ArduCopter\system.cpp

void Copter::init_ardupilot()
{
	init_rangefinder();
}

4.void Copter::init_rangefinder(void)

测距仪初始化。此函数会初始化朝下的测距仪
\ArduCopter\sensors.cpp

void Copter::init_rangefinder(void)
{
#if RANGEFINDER_ENABLED == ENABLED
   rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN);
   rangefinder.init(ROTATION_PITCH_270);
   rangefinder_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt);
   rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);

   // upward facing range finder
   rangefinder_up_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt);
   rangefinder_up_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_90);
#endif
}

5.RangeFinder rangefinder

在 AP_Vehicle.h 文件中，我们用 RangeFinder 类定义了 rangefinder 对象。
\libraries\AP_Vehicle\AP_Vehicle.h

class AP_Vehicle : public AP_HAL::HAL::Callbacks {
{
	protected:
	RangeFinder rangefinder;	
}

6.RangeFinder::init

所以，在跳转 init() 这个成员函数的时候，跳转到 RangeFinder 类的 init() 函数。

初始化 RangeFinder 类。将在这里检测已连接的测距仪。目前还不允许热插拔测距仪。
\libraries\AP_RangeFinder\AP_RangeFinder.cpp

void RangeFinder::init(enum Rotation orientation_default)
{
    if (init_done) {
        // init called a 2nd time?
        return;
    }
    init_done = true;

    convert_params();

    // set orientation defaults
    for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
        params[i].orientation.set_default(orientation_default);
    }

    for (uint8_t i=0, serial_instance = 0; i<RANGEFINDER_MAX_INSTANCES; i++) {
        // serial_instance will be increased inside detect_instance
        // if a serial driver is loaded for this instance
        WITH_SEMAPHORE(detect_sem);
        detect_instance(i, serial_instance);
        if (drivers[i] != nullptr) {
            // we loaded a driver for this instance, so it must be
            // present (although it may not be healthy). We use MAX()
            // here as a UAVCAN rangefinder may already have been
            // found
            num_instances = MAX(num_instances, i+1);
        }

        // initi

7.RangeFinder::detect_instance

检测是否连接了测距仪实例。

根据在文章开始介绍的 RNGFND1_TYPE 参数，选择不同传感器进行检测。

有三种通信后端可以选择
串口后端
IIC后端
SPI后端

此处，以 RNGFND1_TYPE（16）中 AP_RangeFinder_VL53L0X 为例介绍I2C后端。
前端代码通过指定IIC设备的地址而对IIC实例对象进行初始化，初始化代码位于RangeFinder.cpp文件中的RangeFinder::detect_instance函数中：

其中，hal.i2c_mgr->get_device(…)
通过指定IIC地址在总线上得到对应设备

\libraries\AP_RangeFinder\AP_RangeFinder.cpp

void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
{
    const Type _type = (Type)params[instance].type.get();
    switch (_type) {
    case Type::PLI2C:
    case Type::PLI2CV3:
    case Type::PLI2CV3HP:
        FOREACH_I2C(i) {
            if (_add_backend(AP_RangeFinder_PulsedLightLRF::detect(i, state[instance], params[instance], _type),
                             instance)) {
                break;
            }
        }
        break;
    ...
    //I2C后端
    case Type::VL53L0X:
    case Type::VL53L1X_Short:
            FOREACH_I2C(i) {
                if (_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance],
                                                                hal.i2c_mgr->get_device(i, params[instance].address)),
                        instance)) {
                    break;
                }
                if (_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], params[instance],
                                                                hal.i2c_mgr->get_device(i, params[instance].address),
                                                                _type == Type::VL53L1X_Short ?  AP_RangeFinder_VL53L1X::DistanceMode::Short :
                                                                AP_RangeFinder_VL53L1X::DistanceMode::Long),
                                 instance)) {
                    break;
                }
            }
        break;
   ...
  
    
    //串口后端
   case Type::LWSER:
        if (AP_RangeFinder_LightWareSerial::detect(serial_instance)) {
            _add_backend(new AP_RangeFinder_LightWareSerial(state[instance], params[instance]), instance, serial_instance++);
        }

三、后端调用

1.I2C后端

此处，以 RNGFND1_TYPE（16）中 AP_RangeFinder_VL53L0X 为例介绍I2C后端。

1.1 hal.i2c_mgr->get_device

(1)
\libraries\AP_HAL\I2CDevice.h

class I2CDeviceManager {
public:
    /* Get a device handle */
    virtual OwnPtr<AP_HAL::I2CDevice> get_device(uint8_t bus, uint8_t address,
                                                 uint32_t bus_clock=400000,
                                                 bool use_smbus = false,
                                                 uint32_t timeout_ms=4) = 0;
    /*
     * Get device by looking up the I2C bus on the buses from @devpaths.
     *
     * Each string in @devpaths are possible locations for the bus. How the
     * strings are implemented are HAL-specific. On Linux this is the info
     * returned by 'udevadm info -q path /dev/i2c-X'. The first I2C bus
     * matching a prefix in @devpaths is used to create a I2CDevice object.
     */
    virtual OwnPtr<I2CDevice> get_device(std::vector<const char *> devpaths,
                                         uint8_t address) {
        // Not implemented
        return nullptr;
    }

(2)

\libraries\AP_HAL_Linux\I2CDevice.cpp
如果当前的 bus 没有超过 i2c_bus_desc，会 new 一个新的 I2CDevice 类对象返回。

AP_HAL::OwnPtr<AP_HAL::I2CDevice>
I2CDeviceManager::get_device(std::vector<const char *> devpaths, uint8_t address)
{
    const char *dirname = "/sys/class/i2c-dev/";
    struct dirent *de = nullptr;
    DIR *d;

    d = opendir(dirname);
    if (!d) {
        AP_HAL::panic("Could not get list of I2C buses");
    }

    for (de = readdir(d); de; de = readdir(d)) {
        char *str_device, *abs_str_device;
        const char *p;

        if (strcmp(de->d_name, ".") == 0 || strcmp(de->d_name, "..") == 0) {
            continue;
        }

        if (asprintf(&str_device, "%s/%s", dirname, de->d_name) < 0) {
            continue;
        }

        abs_str_device = realpath(str_device, nullptr);
        if (!abs_str_device || !(p = startswith(abs_str_device, "/sys/devices/"))) {
            free(abs_str_device);
            free(str_device);
            continue;
        }

        auto t = std::find_if(std::begin(devpaths), std::end(devpaths),
                              [p](const char *prefix) {
                                  return startswith(p, prefix) != nullptr;
                              });

        free(abs_str_device);
        free(str_device);

        if (t != std::end(devpaths)) {
            unsigned int n;

            /* Found the bus, try to create the device now */
            if (sscanf(de->d_name, "i2c-%u", &n) != 1) {
                AP_HAL::panic("I2CDevice: can't parse %s", de->d_name);
            }
            if (n > UINT8_MAX) {
                AP_HAL::panic("I2CDevice: bus with number n=%u higher than %u",
                              n, UINT8_MAX);
            }

            closedir(d);
            return get_device(n, address);
        }
    }

1.2.AP_RangeFinder_VL53L0X::detect(…)

检测是否连接了 VL53L0X 测距仪。我们将通过 I2C 读取数据来进行检测。如果得到结果，则表示传感器已连接。

首先会根据传入的参数，new 一个 AP_RangeFinder_VL53L0X 类对象。

然后读取测距仪固有的产品 ID 是否正确(sensor->check_id())，再对测距仪进行必要的初始化(sensor->init())。

\libraries\AP_RangeFinder\AP_RangeFinder_VL53L0X.cpp

AP_RangeFinder_Backend *AP_RangeFinder_VL53L0X::detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
{
	if (!dev) {
		return nullptr;
	}
    AP_RangeFinder_VL53L0X *sensor
        = new AP_RangeFinder_VL53L0X(_state, _params, std::move(dev));

    if (!sensor) {
        delete sensor;
        return nullptr;
    }

    sensor->dev->get_semaphore()->take_blocking();
    
    if (!sensor->check_id() || !sensor->init()) {
        sensor->dev->get_semaphore()->give();
        delete sensor;
        return nullptr;
    }

    sensor->dev->get_semaphore()->give();
    
    return sensor;
}

在7中的void RangeFinder::detect_instance函数中调用AP_RangeFinder_VL53L0X::detect函数

1.3 bool AP_RangeFinder_VL53L0X::check_id(void)

检查测距仪 ID 寄存器。每种类型测距仪的 ID 寄存器都有唯一值。

bool AP_RangeFinder_VL53L0X::check_id(void)
{
    uint8_t v1, v2;
    if (!dev->read_registers(0xC0, &v1, 1) ||
        !dev->read_registers(0xC1, &v2, 1) ||
        v1 != 0xEE ||
        v2 != 0xAA) {
        return false;
    }
    printf("Detected VL53L0X on bus 0x%x\n", dev->get_bus_id());
    return true;
}

1.4 bool AP_RangeFinder_VL53L0X::init()

初始化传感器，并注册测距仪的周期运行函数 timer()。

bool AP_RangeFinder_VL53L0X::init() {
{
	...
    通过I2C向传感器寄存器写入初始值。
    ...
 
    // call timer() every 50ms. We expect new data to be available every 50ms
    dev->register_periodic_callback(50000, FUNCTOR_BIND_MEMBER(&AP_RangeFinder_VL53L3CX::timer, void));
 
    return true;

}

1.5 void AP_RangeFinder_VL53L0X::timer(void)

20Hz 调用一次该函数，读取测距仪的测量值。

void AP_RangeFinder_VL53L0X::timer(void)
{
    uint16_t range_mm;
    if (get_reading(range_mm) && range_mm < 8000) {
        sum_mm += range_mm;
        counter++;
    }
}

1.6 AP_RangeFinder_VL53L0X::get_reading函数

// read - return last value measured by sensor
bool AP_RangeFinder_VL53L0X::get_reading(uint16_t &reading_mm)
{
    if ((read_register(RESULT_INTERRUPT_STATUS) & 0x07) == 0) {
        if (AP_HAL::millis() - start_ms > 200) {
            start_continuous();
        }
        return false;
    }

    // assumptions: Linearity Corrective Gain is 1000 (default);
    // fractional ranging is not enabled
    reading_mm = read_register16(RESULT_RANGE_STATUS + 10);
    write_register(SYSTEM_INTERRUPT_CLEAR, 0x01);

    return true;
}

2.串口后端

此处以获取LightWare数据，为例介绍串口后端。

2.1 RangeFinder::_add_backend函数

\libraries\AP_RangeFinder\AP_RangeFinder.cpp

bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend, uint8_t instance, uint8_t serial_instance)
{
    if (!backend) {
        return false;
    }
    if (instance >= RANGEFINDER_MAX_INSTANCES) {
        AP_HAL::panic("Too many RANGERS backends");
    }
    if (drivers[instance] != nullptr) {
        // we've allocated the same instance twice
        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
    }
    backend->init_serial(serial_instance);
    drivers[instance] = backend;
    num_instances = MAX(num_instances, instance+1);

    return true;
}

2.2 AP_RangeFinder_Backend_Serial::init_serial函数

此处以获取LightWare数据为例，首先需通过serial_manager类和用户设置的参数获取串口设备对象实例

\libraries\AP_RangeFinder\AP_RangeFinder_Backend_Serial.cpp

void AP_RangeFinder_Backend_Serial::init_serial(uint8_t serial_instance)
{
    uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
    if (uart != nullptr) {
        uart->begin(initial_baudrate(serial_instance), rx_bufsize(), tx_bufsize());
    }
}

2.3 AP_RangeFinder_Backend_Serial::detect函数

\libraries\AP_RangeFinder\AP_RangeFinder_Backend_Serial.cpp

bool AP_RangeFinder_Backend_Serial::detect(uint8_t serial_instance)
{
    return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
}

2.4 AP_SerialManager::have_serial函数

\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;
}

2.5 AP_SerialManager::UARTState *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;
}

3.SPI后端

    ....

四、前端调用

1.前端

1.1 SCHED_TASK(read_rangefinder, 20, 100, 33)函数
Copter.cpp 文件的周期任务列表中注册了一个线程：SCHED_TASK(read_rangefinder, 20, 100, 33)
注册了调用频率为 20Hz 的 read_rangefinder() 函数。
在\ArduCopter\Copter.cpp中

const AP_Scheduler::Task Copter::scheduler_tasks[] = {
	...
	SCHED_TASK(read_rangefinder,      20,    100,  33),
	...
}

1.2 read_rangefinder函数
在 sensors.cpp 文件中，read_rangefinder()调用rangefinder.update()函数
以厘米为单位返回测距仪高度
sensors.cpp文件中包含有调用设备驱动前端代码，例如飞控控制以20HZ的频率调用read_rangefinder()函数而读取高度数据，而该函数内部则调用了rangefinder.update()前端代码来获取数据

在\ArduCopter\sensors.cpp中

void Copter::read_rangefinder(void)
{
#if RANGEFINDER_ENABLED == ENABLED
    //通过传感器更新高度数据
    rangefinder.update();
    ...
}

1.3 RangeFinder::update函数

update实现代码在：
\libraries\AP_RangeFinder\AP_RangeFinder.cpp中

请添加图片描述

1.4 调用传感器类中的update函数

(1)
AP_RangeFinder_VL53L0X 对象
根据返回的 AP_RangeFinder_VL53L0X 对象，调用 AP_RangeFinder_VL53L0CX 类中的 update() 函数。

\libraries\AP_RangeFinder\AP_RangeFinder_VL53L0X.cpp中

void AP_RangeFinder_VL53L0X::update(void)
{
    if (counter > 0) {
        state.distance_m = (sum_mm * 0.001f) / counter;
        state.last_reading_ms = AP_HAL::millis();
        sum_mm = 0;
        counter = 0;
        update_status();
    } else {
        set_status(RangeFinder::Status::NoData);
    }
}

(2)
如果返回的为AP_RangeFinder_LightWareSerial，调用 AP_RangeFinder_Backend_Serial中的 update() 函数。
libraries\AP_RangeFinder\AP_RangeFinder_Backend_Serial.cpp中

void AP_RangeFinder_Backend_Serial::update(void)
{
	//获取缓冲区中获取的原始数据，并且将处理后的数据保存至distance_cm中，数据为true，否则为false
    if (get_reading(state.distance_m)) {
        // update range_valid state based on distance measured
        state.last_reading_ms = AP_HAL::millis();//获取当前系统运行时间
        //判断distance_cm数据情况，高于最大测量范围或者小于最小测量范围或者数据正常
        update_status();
    	 // 超过200ms缓冲没有数据 
    } else if (AP_HAL::millis() - state.last_reading_ms > read_timeout_ms()) {
        set_status(RangeFinder::Status::NoData);
    }
}

前端代码在读取串口数据之前，需每次调用update()方法获取串口接受缓冲区中的数据，update方法中则调用的get_reading()方法将数据读取的内存中进行数据处理。其中关于update的代码可见之前的rangefinder.update()代码的实现，另外get_reading()代码如下：
进入AP_RangeFinder_LightWareSerial::get_reading函数
\libraries\AP_RangeFinder\AP_RangeFinder_LightWareSerial.cpp

// read - return last value measured by sensor
bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m)
{
    if (uart == nullptr) {
        return false;
    }

    float sum = 0;              // sum of all readings taken
    uint16_t valid_count = 0;   // number of valid readings
    uint16_t invalid_count = 0; // number of invalid readings

    // max distance the sensor can reliably measure - read from parameters
    const int16_t distance_cm_max = max_distance_cm();

    // read any available lines from the lidar
    //检测串口接收缓冲区中的数据个数
    int16_t nbytes = uart->available();
    //将缓冲区的数据读出，可能会读到多组数据
    while (nbytes-- > 0) {
        //获取一个字符
        char c = uart->read();
        // use legacy protocol
        if (protocol_state == ProtocolState::UNKNOWN || protocol_state == ProtocolState::LEGACY) {
            //一组数据以'\r'为结尾
            if (c == '\r') {
                linebuf[linebuf_len] = 0;
                const float dist = strtof(linebuf, nullptr);
                if (!is_negative(dist) && !is_lost_signal_distance(dist * 100, distance_cm_max)) {
                    sum += dist;
                    valid_count++;
                    // if still determining protocol update legacy valid count
                    if (protocol_state == ProtocolState::UNKNOWN) {
                        legacy_valid_count++;
                    }
                } else {
                    invalid_count++;
                }
                linebuf_len = 0;
            //判断数据是否有效
            } else if (isdigit(c) || c == '.' || c == '-') {
                linebuf[linebuf_len++] = c;
                if (linebuf_len == sizeof(linebuf)) {
                    // too long, discard the line
                    linebuf_len = 0;
                }
            }
        }
        // use binary protocol
        if (protocol_state == ProtocolState::UNKNOWN || protocol_state == ProtocolState::BINARY) {
            bool msb_set = BIT_IS_SET(c, 7);
            if (msb_set) {
                // received the high byte
                high_byte = c;
                high_byte_received = true;
            } else {
                // received the low byte which should be second
                if (high_byte_received) {
                    const int16_t dist = (high_byte & 0x7f) << 7 | (c & 0x7f);
                    if (dist >= 0 && !is_lost_signal_distance(dist, distance_cm_max)) {
                        sum += dist * 0.01f;
                        valid_count++;
                        // if still determining protocol update binary valid count
                        if (protocol_state == ProtocolState::UNKNOWN) {
                            binary_valid_count++;
                        }
                    } else {
                        invalid_count++;
                    }
                }
                high_byte_received = false;
            }
        }
    }
    // protocol set after 10 successful reads
    if (protocol_state == ProtocolState::UNKNOWN) {
        if (binary_valid_count > 10) {
            protocol_state = ProtocolState::BINARY;
        } else if (legacy_valid_count > 10) {
            protocol_state = ProtocolState::LEGACY;
        }
    }

    uint32_t now = AP_HAL::millis();
    if (last_init_ms == 0 ||
        (now - last_init_ms > 1000 &&
         now - state.last_reading_ms > 1000)) {
        // send enough serial transitions to trigger LW20 into serial
        // mode. It starts in dual I2C/serial mode, and wants to see
        // enough transitions to switch into serial mode.
        uart->write("www\r\n");
        last_init_ms = now;
    } else {
        uart->write('d');
    }

    // return average of all valid readings
    if (valid_count > 0) {
        reading_m = sum / valid_count;
        no_signal = false;
        return true;
    }

    // all readings were invalid so return out-of-range-high value
    if (invalid_count > 0) {
        reading_m = MIN(MAX(LIGHTWARE_DIST_MAX_CM, distance_cm_max + LIGHTWARE_OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f;
        no_signal = true;
        return true;
    }

    // no readings so return false
    return false;
}