调试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() 函数。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)