Ardupilot — AP_RangeFinder代码梳理

简介: Ardupilot — AP_RangeFinder代码梳理

前言

故事的开始,要从参数 RNGFND1_TYPE 说起。

RNGFND1_TYPE:测距仪类型

连接的测距仪类型。


Values

Value

Meaning

0

None

1

Analog

2

MaxbotixI2C

3

LidarLite-I2C

5

PWM

6

BBB-PRU

7

LightWareI2C

8

LightWareSerial

9

Bebop

10

MAVLink

11

USD1_Serial

12

LeddarOne

13

MaxbotixSerial

14

TeraRangerI2C

15

LidarLiteV3-I2C

16

VL53L0X or VL53L1X

17

NMEA

18

WASP-LRF

19

BenewakeTF02

20

Benewake-Serial

21

LidarLightV3HP

22

PWM

23

BlueRoboticsPing

24

DroneCAN

25

BenewakeTFminiPlus-I2C

26

LanbaoPSK-CM8JL65-CC5

27

BenewakeTF03

28

VL53L1X-ShortRange

29

LeddarVu8-Serial

30

HC-SR04

31

GYUS42v2

32

MSP

33

USD1_CAN

34

Benewake_CAN

35

TeraRangerSerial

36

Lua_Scripting

37

NoopLoop_TOFSense

100

SITL

显示详细信息

本文主要梳理,Ardupilot 中测距仪的代码运行流程,以及如何选择相应的测距仪类型。

1 Copter.cpp

1.1 void Copter::setup()

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

void Copter::setup()
{
    // Load the default values of variables listed in var_info[]s
    AP_Param::setup_sketch_defaults();
 
    // setup storage layout for copter
    StorageManager::set_layout_copter();
 
    init_ardupilot();
 
    // initialise the main loop scheduler
    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
}

2 system.cpp

2.1 void Copter::init_ardupilot()

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

void Copter::init_ardupilot()
{
    ...
 
    // initialise rangefinder
    init_rangefinder();
 
    ...
}

3 sensors.cpp

3.1 void Copter::init_rangefinder(void)

测距仪初始化。此函数会初始化朝下的测距仪。

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(RANGEFINDER_WPNAV_FILT_HZ);
   rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);
 
   // upward facing range finder
   rangefinder_up_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
   rangefinder_up_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_90);
#endif
}

3.2 对象rangefinder说明

AP_Vehicle.h 文件中,我们用 RangeFinder 类定义了 rangefinder 对象。

    // sensor drivers
    AP_GPS gps;
    AP_Baro barometer;
    Compass compass;
    AP_InertialSensor ins;
    AP_Button button;
    RangeFinder rangefinder;

4 RangeFinder.cpp

4.1 void RangeFinder::init(enum Rotation orientation_default)

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

初始化 RangeFinder 类。我们将在这里检测已连接的测距仪。目前我们还不允许热插拔测距仪。

/*
  initialise the RangeFinder class. We do detection of attached range
  finders here. For now we won't allow for hot-plugging of
  rangefinders.
 */
void RangeFinder::init(enum Rotation orientation_default)
{
    if (num_instances != 0) {
        // init called a 2nd time?
        return;
    }
 
    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
        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)
            num_instances = i+1;
        }
 
        // initialise status
        state[i].status = RangeFinder_NotConnected;
        state[i].range_valid_count = 0;
    }
}

4.2 void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)

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

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

此处,我们以 RNGFND1_TYPE(16) AP_RangeFinder_VL53L3CX 为例进行介绍。

/*
  detect if an instance of a rangefinder is connected.
 */
void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
{
    enum RangeFinder_Type _type = (enum RangeFinder_Type)params[instance].type.get();
    switch (_type) {
    case RangeFinder_TYPE_PLI2C:
    case RangeFinder_TYPE_PLI2CV3:
    case RangeFinder_TYPE_PLI2CV3HP:
        FOREACH_I2C(i) {
            if (_add_backend(AP_RangeFinder_PulsedLightLRF::detect(i, state[instance], params[instance], _type))) {
                break;
            }
        }
        break;
    
    ...
 
    case RangeFinder_TYPE_VL53L0X:
            FOREACH_I2C(i) {
                if (_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance],
                                                                 hal.i2c_mgr->get_device(i, params[instance].address)))) {
                    break;
                }
 
                if (_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], params[instance],
                                                                hal.i2c_mgr->get_device(i, params[instance].address)))) {
                    break;
                }
 
                if (_add_backend(AP_RangeFinder_VL53L3CX::detect(state[instance], params[instance],
                                                                hal.i2c_mgr->get_device(i, params[instance].address)))) {
                    break;
                }
            }
        break;
 
    ...
}

5 I2CDevice.cpp

5.1 hal.i2c_mgr->get_device(i, params[instance].address)

如果当前的 bus 没有超过 i2c_bus_desc,会 new 一个新的 I2CDevice 类对象返回。

AP_HAL::OwnPtr<AP_HAL::I2CDevice>
I2CDeviceManager::get_device(uint8_t bus, uint8_t address,
                             uint32_t bus_clock,
                             bool use_smbus,
                             uint32_t timeout_ms)
{
    if (bus >= ARRAY_SIZE(i2c_bus_desc)) {
        return AP_HAL::OwnPtr<AP_HAL::I2CDevice>(nullptr);
    }
    auto dev = AP_HAL::OwnPtr<AP_HAL::I2CDevice>(new I2CDevice(bus, address, bus_clock, use_smbus, timeout_ms));
    return dev;
}

6 AP_RangeFinder_VL53L3CX.cpp

6.1 AP_RangeFinder_Backend *AP_RangeFinder_VL53L3CX::detect(...)

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

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

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

/*
   detect if a VL53L3CX rangefinder is connected. We'll detect by
   trying to take a reading on I2C. If we get a result the sensor is
   there.
*/
AP_RangeFinder_Backend *AP_RangeFinder_VL53L3CX::detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
{
    if (!dev) {
        return nullptr;
    }
 
    hal.console->printf("VL53L3CX: detect...\n");
 
    AP_RangeFinder_VL53L3CX *sensor = new AP_RangeFinder_VL53L3CX(_state, _params, std::move(dev));
 
    if (!sensor) {
        delete sensor;
        hal.console->printf("VL53L3CX: delete sensor...\n");
        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;
}

6.2 bool AP_RangeFinder_VL53L3CX::check_id(void)

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

// check sensor ID registers
bool AP_RangeFinder_VL53L3CX::check_id(void)
{
    uint8_t v1 = 0;
    uint8_t v2 = 0;
    if (!(read_register(0x010F, v1) && read_register(0x0110, v2))) {
        hal.console->printf("VL53L3CX: v1=0x%02x  v2=0x%02x\n", v1, v2);
        return false;
    }
 
    if ((v1 != 0xEA) || (v2 != 0xAA)) {
        hal.console->printf("VL53L3CX: Not a recognised device.\n");
        return false;
    }
 
    printf("Detected VL53L3CX on bus 0x%x.\n", dev->get_bus_id());
    return true;
}

6.3 bool AP_RangeFinder_VL53L3CX::init()

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

/*
  initialise sensor
 */
bool AP_RangeFinder_VL53L3CX::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;
}

6.4 void AP_RangeFinder_VL53L3CX::timer(void)

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

/*
  timer called at 20Hz
*/
void AP_RangeFinder_VL53L3CX::timer(void)
{
    uint16_t range_mm;
    if ((get_reading(range_mm)) && (range_mm <= 4000)) {
        WITH_SEMAPHORE(_sem);
        sum_mm += range_mm;
        // printf("Range_mm: %dmm\n", range_mm);
        counter++;
    }
}

6.5 SCHED_TASK(read_rangefinder,      20,    100)

Copter.cpp 文件的周期任务列表中,注册了调用频率为 20Hzread_rangefinder() 函数。

const AP_Scheduler::Task Copter::scheduler_tasks[] = {
...
 
#if RANGEFINDER_ENABLED == ENABLED
    SCHED_TASK(read_rangefinder,      20,    100),
#endif
...
}

6.6 void Copter::read_rangefinder(void)

sensors.cpp 文件中,以厘米为单位返回测距仪高度。

// return rangefinder altitude in centimeters
void Copter::read_rangefinder(void)
{
#if RANGEFINDER_ENABLED == ENABLED
    rangefinder.update();
...
}

6.7 void AP_RangeFinder_VL53L3CX::update(void)

根据 6.1 中的返回的 AP_RangeFinder_VL53L3CX 对象,调用 AP_RangeFinder_VL53L3CX 类中的 update() 函数。

/*
   update the state of the sensor
*/
void AP_RangeFinder_VL53L3CX::update(void)
{
    WITH_SEMAPHORE(_sem);
    if (counter > 0) {
        state.distance_cm = sum_mm / (10*counter);
        state.last_reading_ms = AP_HAL::millis();
        update_status();
        sum_mm = 0;
        counter = 0;
        // printf("VL53L3CX: %dcm\n", state.distance_cm);
    } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
        // if no updates for 0.2s set no-data
        set_status(RangeFinder::RangeFinder_NoData);
    }
}


相关文章
|
Java 开发者
使用HashMap的values()方法返回的值转换为List时遇到错误
使用HashMap的values()方法返回的值转换为List时遇到错误
|
Ubuntu Linux
ubuntu打开usb摄像头
ubuntu打开usb摄像头
1342 0
|
2月前
|
传感器 定位技术 数据格式
常用通信协议及数据格式
常用通信协议和格式总结
256 2
|
监控 Windows
(1)Mission Planner概述
(1)Mission Planner概述
797 2
|
10月前
|
机器学习/深度学习 自然语言处理 语音技术
Python在深度学习领域的应用,重点讲解了神经网络的基础概念、基本结构、训练过程及优化技巧
本文介绍了Python在深度学习领域的应用,重点讲解了神经网络的基础概念、基本结构、训练过程及优化技巧,并通过TensorFlow和PyTorch等库展示了实现神经网络的具体示例,涵盖图像识别、语音识别等多个应用场景。
347 8
|
人工智能 自然语言处理 API
深度融合与创新:Open API技术促进AI服务生态构建
【7月更文第21天】在数字化转型的浪潮中,人工智能(AI)已从概念探索走向实际应用,深刻改变着各行各业。Open API(开放应用程序接口)作为连接技术与业务的桥梁,正成为推动AI服务普及和生态构建的关键力量。本文将探讨Open API技术如何通过标准化、易用性和灵活性,加速AI服务的集成与创新,构建一个更加丰富多元的AI服务生态系统。
608 2
|
云安全 存储 人工智能
云启智能新纪元,2024可信云大会亮点抢先看!
2024年可信云大会将于7月23-24日在京举行,聚焦云计算与人工智能融合,探讨智算服务与大模型云服务。
507 0
|
SQL 关系型数据库 MySQL
MySQL数据库——图形化界面工具(DataGrip),SQL(2)-DML(插入、修改和删除数据)
MySQL数据库——图形化界面工具(DataGrip),SQL(2)-DML(插入、修改和删除数据)
1207 1
|
监控 安全 Java
线程死循环定位与处理:精准定位,妥善处理,预防为先
精准定位死循环,妥善处理问题,编码阶段防风险。
408 0