Ardupilot — AP_RangeFinder代码梳理

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 文件的周期任务列表中，注册了调用频率为 20Hz 的 read_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);
    }
}