PX4代码中LIDAR-Lite v3激光测距仪I2C驱动

调试LIDAR-Lite v3激光测距仪，通读了PX4代码中驱动相关的部分。在这里做一下记录。

飞控硬件是pixhawk1，PX4软件版本为1.7.3。

按照官方教程进行接线和配置，链接为 https://docs.px4.io/en/sensor/lidar_lite.html。

首先，确保在 cmake/configs/nuttx_px4fmu-v2_default.cmake 文件中 drivers/device 和 drivers/ll40ls 这两行没有被 # 注释，文件夹下的代码参与编译。其中 drivers/device 文件夹中提供了I2C的代码，drivers/ll40ls 文件夹中提供了LIDAR-Lite激光测距仪设备的驱动代码。

查看 drivers/ll40ls 文件夹中的 ll40ls.cpp 文件，主函数为 ll40ls_main()，针对不同指令参数分别进行执行。在正常开机运行时，启动文件rcS会调用到rc.sensors，其中有：

# Sensors on the PWM interface bank
if param compare SENS_EN_LL40LS 1
then
	if pwm_input start
	then
		ll40ls start pwm
	fi
fi

# Lidar-Lite on I2C
if param compare SENS_EN_LL40LS 2
then
	ll40ls start i2c
fi

由于配置了参数SENS_EN_LL40LS = 2，会执行 ll40ls start i2c 指令，在 ll40ls_main() 函数中会跳转到 ll40ls::start() 函数；建立 LidarLiteI2C 类的一个实例 instance = new LidarLiteI2C()，执行初始化函数 instance->init() 初始化I2C接口、建立循环队列、广告 distance_sensor 消息，ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) 设定运行周期。

在 LidarLiteI2C::ioctl() 函数中，又调用了 LidarLite::ioctl() 和 I2C::ioctl()进行相关配置；而 LidarLite::ioctl() 在设置完运行周期后，直接调用了虚函数 start() ，实际生效的是子类的函数 LidarLiteI2C::start()；将 LidarLiteI2C::cycle_trampoline() 加入work_queue()，延迟为1；在 LidarLiteI2C::cycle_trampoline() 中直接调用 LidarLiteI2C::cycle() ，通过 LidarLiteI2C::collect() 完成一次数据采集；采集成功后，再次将 LidarLiteI2C::cycle_trampoline() 加入work_queue()，按照前后的时间差来计算延迟时间，确保数据采集的周期可以按照设定值进行，采集周期是50ms。

在 LidarLiteI2C::collect() 函数中，通过I2C接口向 LIDAR-Lite 激光测距仪的寄存器发送采集指令，就可以获取到两个字节的返回数据，分别代表数据的高低字节，将数据拼接后就是单位为cm的测距数值， uint16_t distance_cm = (val[0] << 8) | val[1] 。获取测距数值后进行单位转换，并发布 distance_sensor 的uorb消息。

这里使用了一个长度为2的 ringbuffer::RingBuffer *_reports; 循环队列来进行采集数据的存储，每次 collect() 都会向队列中写入一个新数据，其中包含了采集数据的时间；由于长度为2，一旦队列满之后，所有写数据和读数据操作都必然发生在不同的位置，这样就可以保证在不同的线程中可以安全地使用读数据的功能，不会与写数据发生冲突，这点非常精巧。ll40ls.cpp 文件的 test() 函数就是通过调用了 LidarLiteI2C::read() 函数，进而通过 _reports->get() 来获取队列中的数据，而不必加互斥锁来进行线程保护。

 

PS：

APM代码也支持LIDAR-Lite设备，以Copter2.4.6代码为基础进行分析。

在 ArduCopter/Copter.h 头文件中，类 Copter 包含成员变量 AP_BoardConfig BoardConfig; 和 RangeFinder rangefinder; ，其中第一个用来启动 PX4 驱动对LIDAR-Lite设备的支持，第二个是APM自己的对RangeFinder的一个抽象。

在 Copter::setup() 函数中调用 Copter::init_ardupilot()，进而调用 BoardConfig.init()，进而 AP_BoardConfig::px4_setup()，进而 AP_BoardConfig::px4_setup_drivers()，进而 AP_BoardConfig::px4_start_optional_sensors()，进而：

    if (px4_start_driver(ll40ls_main, "ll40ls", "-X start")) {
        printf("Found external ll40ls sensor\n");
    }
    if (px4_start_driver(ll40ls_main, "ll40ls", "-I start")) {
        printf("Found internal ll40ls sensor\n");
    }

采用这种方式来启动LIDAR-Lite设备的线程，与 PX4 脚本执行 ll40ls start i2c 指令效果相同。

同样地，在 Copter::init_ardupilot() 还调用了Copter::init_rangefinder() 运行 rangefinder.init() 来初始化具体设备的实例instance；而在 Copter.cpp 文件中将 Copter::read_rangefinder() 加入到周期任务表 SCHED_TASK(read_rangefinder, 20, 100) ，按照20Hz的运行周期来执行 rangefinder.update() 来更新数据。

实际调用的是 AP_RangeFinder_PX4::update()，可以看到是采用了 ::read() 函数来读取测距数值，使用方式与 ll40ls::test() 相同，也就是异步地读取 ringbuffer::RingBuffer *_reports 循环队列中的数据来作为最新的采集数据。从理论上讲，这种读取方式要比基于消息的方式滞后一些时间。

因此，本质上来说APM代码并没有直接去实现LIDAR-Lite设备的数据获取功能，而是开启了PX4代码中已完成的数据获取功能，然后通过读取其循环队列记录的数据来作为自己获取的数据。

 

PPS：

再补充一点，为什么 ::read() 函数会调用到对应驱动中的 read() 函数呢？

还是以 LidarLiteI2C 类为例，其父类为 I2C 类，I2C 类又继承了 CDev 类，实际上所有的驱动程序都继承 CDev 类。

在进行初始化的时候，CDev::init() 调用了 ret = register_driver(_devname, &fops, 0666, (void *)this); 函数，将设备名称注册到 nuttx 内核的设备节点中，其中 fops 是 CDev 类成员，它里面确定了文件操作的入口，如下所示：

const struct file_operations device::CDev::fops = {
open	: cdev_open,
close	: cdev_close,
read	: cdev_read,
write	: cdev_write,
seek	: cdev_seek,
ioctl	: cdev_ioctl,
poll	: cdev_poll
};

当设备的操作指令比如 ::read() 函数被调用时，实际上会通过 fops 映射到这里的 cdev_read() 函数

static ssize_t
cdev_read(file_t *filp, char *buffer, size_t buflen)
{
	device::CDev *cdev = (device::CDev *)(filp->f_inode->i_private);

	return cdev->read(filp, buffer, buflen);
}

在进行设备注册 register_driver() 时传递的 this 指针就会被取出来保存为 cdev；实际上这个指针指向的是子类 LidarLiteI2C 的 instance 实例；调用 cdev->read() 虚函数，实际上是子类的 LidarLiteI2C:read() 函数被调用。

因此，在APM代码中调用 ::read() 函数实际运行了 LidarLiteI2C:read() 函数。