最近学习了一下SPI的驱动软件,在此将其进行总结。
本文使用的代码为pixhawk 1.5.5版本的源码
源码下载地址
第一步函数入口:
老规矩,所有px4的代码的函数入口都是在启动脚本中,启动脚本地址为
Firmware\ROMFS\px4fmu_common\init.d 目录下 rc.sensors 软件中默认启动不需要做任何修改
if (!strcmp(verb, "start")) {
ms5611::start(busid, device_type == 5607 ? MS5607_DEVICE : MS5611_DEVICE);
}
第二步MS5611与MS5611_SPI的实例化:
start 函数中调用了 start_bus函数,其中包含两个输入参数 bus_options 是ms5611的一些配置信息,如总线协议,设备地址等,device_type是设备型号,芯片名称等。
started = started | start_bus(bus_options[i], device_type);
start_bus 函数的主要功能
1、运行ms5611_spi.cpp的init函数 -- SPI的初始化
2、new MS5611 创建MS5611的类并运行构造函数,并将 read ioctl measured 等虚函数进行重写
3、运行ms5611.cpp 中的init 函数
4、open 设备地址,并通过句柄执行ioctl 函数
/**
* Start the driver.
*/
bool
start_bus(struct ms5611_bus_option &bus, enum MS56XX_DEVICE_TYPES device_type)
{
if (bus.dev != nullptr) {
errx(1, "bus option already started");
}
prom_u prom_buf;
device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum);
if (interface->init() != OK) { //调用ms5611_spi.cpp 的初始化函数
delete interface;
warnx("no device on bus %u", (unsigned)bus.busid);
return false;
}
/*创建对象 MS5611 重构虚函数,运行构造函数*/
bus.dev = new MS5611(interface, prom_buf, bus.devpath, device_type);
if (bus.dev != nullptr && OK != bus.dev->init()) {
delete bus.dev;
bus.dev = NULL;
return false;
}
/*打开设备*/
int fd = open(bus.devpath, O_RDONLY);
/* set the poll rate to default, starts automatic data collection */
if (fd == -1) {
errx(1, "can't open baro device");
}
/* ioctl 入口,函数入口-开启循环 */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
close(fd);
errx(1, "failed setting default poll rate");
}
close(fd);
return true;
}
第三步:ms5611_spi的初始化
在ms5611_spi.cpp 中,首先对SPI接口进行了配置 SPI::init() --工作模式,频率等
然后对ms5611进行了reset,并尝试是否能读取ms5611的PROM。
int
MS5611_SPI::init()
{
int ret;
ret = SPI::init();
if (ret != OK) {
DEVICE_DEBUG("SPI init failed");
goto out;
}
/* send reset command */
ret = _reset();
if (ret != OK) {
DEVICE_DEBUG("reset failed");
goto out;
}
/* read PROM */
ret = _read_prom();
if (ret != OK) {
DEVICE_DEBUG("prom readout failed");
goto out;
}
out:
return ret;
}
第四步:ms5611类的实例化并运行init函数
本段代码,创建了环形缓冲区,并发送采集命令,获取采集后的数据
int
MS5611::init()
{
int ret;
ret = CDev::init();
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s));
if (_reports == nullptr) {
DEVICE_DEBUG("can't get memory for reports");
ret = -ENOMEM;
goto out;
}
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(BARO_BASE_DEVICE_PATH);
struct baro_report brp;
/* do a first measurement cycle to populate reports with valid data */
_measure_phase = 0;
_reports->flush();
/* this do..while is goto without goto */
do {
/* do temperature first */
if (OK != measure()) {
ret = -EIO;
break;
}
usleep(MS5611_CONVERSION_INTERVAL);
if (OK != collect()) {
ret = -EIO;
break;
}
/* now do a pressure measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
usleep(MS5611_CONVERSION_INTERVAL);
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
_reports->get(&brp);
ret = OK;
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
if (_baro_topic == nullptr) {
warnx("failed to create sensor_baro publication");
}
} while (0);
out:
return ret;
}
第五段:工作队列进入循环,开始采集
先open设备地址,然后运行ioctl。最后进入work_queue函数,开始循环采集并通过orb将其发送出来。
int fd = open(bus.devpath, O_RDONLY); //通过地址打开设备获取句柄
/* set the poll rate to default, starts automatic data collection */
if (fd == -1) {
errx(1, "can't open baro device");
}
//运行ioctl,从这里进入循环
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
close(fd);
errx(1, "failed setting default poll rate");
}
close(fd);
总结:大致的ms5611驱动的流程就是这样,注意区分各个init函数的调用。这里用了各种类与继承等,还有命名空间等,所以出现了好多同名或大小写不一样的函数,有时候就会找错地方。这里一定要注意。
1、先初始化SPI的接口 --ms5611_spi.cpp
2、再初始化ms5611模块 --ms5611.cpp
3、通过work_queue 函数进行循环采集。