前言
故事的开始，要从参数 RNGFND1_TYPE 说起。
RNGFND1_TYPE：测距仪类型
连接的测距仪类型。
Values |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
显示详细信息
本文主要梳理，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); } }
