欢迎交流~ 个人 Gitter 交流平台,点击直达:
要想自己添加一个传感器的话,最好先搞明白已有的传感器的工作过程。
这里记录一下PX4中MPU6000加速度计陀螺仪的解读过程,从mpu6000.cpp出发,介绍从驱动注册到原始数据读取的过程。涉及到一些关于Linux设备驱动开发的知识。
在继续往下读之前有必要先感受一下PX4中驱动的注册过程,以及关键的设备驱动ID分配。
在NuttX操作系统中,MPU6000是以字符设备的形式存在的,这一点从MPU6000这个类的定义中可用看出来
class MPU6000 : public device::CDev{ }
MPU6000类以公有形式继承自CDev(character device)字符型设备,表明MPU6000可以看做成是字符型设备,可以进行如下的设备操作
const struct file_operations CDev::fops = {
open : cdev_open,
close : cdev_close,
read : cdev_read,
write : cdev_write,
seek : cdev_seek,
ioctl : cdev_ioctl,
poll : cdev_poll,
};
这里最值得关注的是file_operations
这个结构体,其定义位于fs.h
,该文件中包含所有字符型设备的结构体和API。在Linux系统中,万物皆文件,所有的设备都被当做文件进行操作open、read、close等。
struct file_operations
{
int (*open)(FAR struct file *filp);
int (*close)(FAR struct file *filp);
ssize_t (*read)(FAR struct file *filp, FAR char *buffer, size_t buflen);
ssize_t (*write)(FAR struct file *filp, FAR const char *buffer, size_t buflen);
off_t (*seek)(FAR struct file *filp, off_t offset, int whence);
int (*ioctl)(FAR struct file *filp, int cmd, unsigned long arg);
#ifndef CONFIG_DISABLE_POLL
int (*poll)(FAR struct file *filp, struct pollfd *fds, bool setup);
#endif
};
按照这样的思路,大可以想象直接将传感器作为一个文件open然后read即可,思路是正确的,但是需要一些前提条件的:
- 驱动注册。只有将设备注册到系统中才能进行文件操作的,而且怎么保证你打开的设备是你想要打开的?MPU6000的加速度计、陀螺仪算是两个设备了
- 端口配置。MPU6000通过SPI总线连接,中断读取,要配置的东西还是有一些的。
board.h
中介绍了Pixhawk飞控板的资源分配情况,包括STM32的时钟配置,各串口的引脚对应情况,I2C、CAN的连接以及本文着重要关注的SPI总线连接情况:
/*
* SPI
*
* There are sensors on SPI1, and SPI2 is connected to the FRAM.
*/
#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) // SPI1
#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz)
#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) // SPI2
#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz)
#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz)
#define GPIO_SPI4_MISO (GPIO_SPI4_MISO_1|GPIO_SPEED_50MHz) // SPI4
#define GPIO_SPI4_MOSI (GPIO_SPI4_MOSI_1|GPIO_SPEED_50MHz)
#define GPIO_SPI4_SCK (GPIO_SPI4_SCK_1|GPIO_SPEED_50MHz)
Pixhawk飞控板引出了3个SPI总线接口
在文件board_config.h
中则是对相关引脚的功能配置,例如给PWM舵机输出引脚上拉、定时器配置、ADC定义以及关键的SPI总线设置。主要包括:
/* SPI chip selects */
// SPI芯片片选引脚配置
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
#define GPIO_SPI_CS_HMC (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN1)
#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
/////// 外部扩展SPI4的片选引脚
#define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
#define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
#define GPIO_SPI_CS_EXT2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_EXT3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SPI_CS_LIS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
#define PX4_SPI_BUS_SENSORS 1
#define PX4_SPI_BUS_RAMTRON 2
#define PX4_SPI_BUS_EXT 4
#define PX4_SPI_BUS_BARO PX4_SPI_BUS_SENSORS
一共三各SPI接口1、2、4,其中传感器连到SPI1上,铁电随机存储器FM25V01连到SPI2上,还有外部SPI4。
**注意:**FMUv3也就是常说的Pixhawk2.1的Cube中有两套IMU,用的就是SPI4,并且外接的两套IMU与Pixhawk上原有的两套IMU是相同的,Pixhawk2上多出来一套MPU9250九轴IMU不知道用上没有。
#define PX4_SPIDEV_GYRO 1
#define PX4_SPIDEV_ACCEL_MAG 2
#define PX4_SPIDEV_BARO 3
#define PX4_SPIDEV_MPU 4
#define PX4_SPIDEV_HMC 5
#define PX4_SPIDEV_LIS 7
#define PX4_SPIDEV_BMI 8
如同fs.h
中包含了所有字符型设备的结构体和API,spi.h
中是所有SPI设备驱动和API的定义。
需要注意spi_ops_s这个关键的指向函数的结构体,SPI协议的相关操作都可以从这里找到:select(片选)、setmode(时钟极性、相位)、setbit(8/16位)等等。
struct spi_ops_s
{
#ifndef CONFIG_SPI_OWNBUS
int (*lock)(FAR struct spi_dev_s *dev, bool lock);
#endif
void (*select)(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
bool selected);
uint32_t (*setfrequency)(FAR struct spi_dev_s *dev, uint32_t frequency);
void (*setmode)(FAR struct spi_dev_s *dev, enum spi_mode_e mode);
void (*setbits)(FAR struct spi_dev_s *dev, int nbits);
uint8_t (*status)(FAR struct spi_dev_s *dev, enum spi_dev_e devid);
#ifdef CONFIG_SPI_CMDDATA
int (*cmddata)(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool cmd);
#endif
uint16_t (*send)(FAR struct spi_dev_s *dev, uint16_t wd);
#ifdef CONFIG_SPI_EXCHANGE
void (*exchange)(FAR struct spi_dev_s *dev, FAR const void *txbuffer,
FAR void *rxbuffer, size_t nwords);
#else
void (*sndblock)(FAR struct spi_dev_s *dev, FAR const void *buffer,
size_t nwords);
void (*recvblock)(FAR struct spi_dev_s *dev, FAR void *buffer,
size_t nwords);
#endif
int (*registercallback)(FAR struct spi_dev_s *dev, spi_mediachange_t callback,
void *arg);
};
文件stm32_spi.c
中是spi协议的函数实现。
首先以SPI1为例,g_sp1iops
是一个spi_ops_s结构体,可以类似的理解为SPI这个类的实例,包含了所有的成员.select、setmode等,并且对应的完成了功能函数的实现,如spi_setfrequency、spi_setmode等。stm32_spi1select的实现在后面的文件中会介绍。
#ifdef CONFIG_STM32_SPI1
static const struct spi_ops_s g_sp1iops =
{
#ifndef CONFIG_SPI_OWNBUS
.lock = spi_lock,
#endif
.select = stm32_spi1select,
.setfrequency = spi_setfrequency,
.setmode = spi_setmode,
.setbits = spi_setbits,
.status = stm32_spi1status,
#ifdef CONFIG_SPI_CMDDATA
.cmddata = stm32_spi1cmddata,
#endif
.send = spi_send,
#ifdef CONFIG_SPI_EXCHANGE
.exchange = spi_exchange,
#else
.sndblock = spi_sndblock,
.recvblock = spi_recvblock,
#endif
.registercallback = 0,
};
static struct stm32_spidev_s g_spi1dev =
{
.spidev = { &g_sp1iops },
.spibase = STM32_SPI1_BASE,
.spiclock = STM32_PCLK2_FREQUENCY,
#ifdef CONFIG_STM32_SPI_INTERRUPTS
.spiirq = STM32_IRQ_SPI1,
#endif
#ifdef CONFIG_STM32_SPI_DMA
.rxch = DMACHAN_SPI1_RX,
.txch = DMACHAN_SPI1_TX,
#endif
};
#endif
由结构体g_spi1dev
的第一个成员.spidev = { &g_sp1iops }
可以看出结构体g_sp1iops
是属于g_spi1dev
这个结构体的,因此一个spi设备可以由stm32_spidev_s这个结构体的实例表示。
本文件中有几个关键的函数需要注意:
/* 将所选的SPI端口初始化为其默认状态 */
static void spi_portinitialize(FAR struct stm32_spidev_s *priv)
{
/* Configure CR1. Default configuration:
* Mode 0: CPHA=0 and CPOL=0
* Master: MSTR=1
* 8-bit: DFF=0
* MSB tranmitted first: LSBFIRST=0
* Replace NSS with SSI & SSI=1: SSI=1 SSM=1 (prevents MODF error)
* Two lines full duplex: BIDIMODE=0 BIDIOIE=(Don't care) and RXONLY=0
*/
...
}
/* 初始化spi端口 */
FAR struct spi_dev_s *up_spiinitialize(int port)
{
// 以SPI1为例
#ifdef CONFIG_STM32_SPI1
if (port == 1) // 对应硬件配置中的宏定义
{
/* Select SPI1 */
priv = &g_spi1dev;
/* Only configure if the port is not already configured */
if ((spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0)
{
/* Configure SPI1 pins: SCK, MISO, and MOSI */
// 配置SPI1的SCK、MISO、MOSI引脚
stm32_configgpio(GPIO_SPI1_SCK);
stm32_configgpio(GPIO_SPI1_MISO);
stm32_configgpio(GPIO_SPI1_MOSI);
/* Set up default configuration: Master, 8-bit, etc. */
// 设置SPI默认配置
spi_portinitialize(priv); // 上面的函数
}
}
else
#endif
#ifdef CONFIG_STM32_SPI2
....
}
文件px4fmu_spi.c
中是一些Pixhawk飞控板特定的SPI函数
/* 为PX4FMU板配置SPI片选GPIO引脚 */
__EXPORT void stm32_spiinitialize(void)
{
#ifdef CONFIG_STM32_SPI1
px4_arch_configgpio(GPIO_SPI_CS_GYRO); // PC13 L3GD20陀螺仪片选
px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG); // PC15 LSM303D加速度计/磁力计片选
px4_arch_configgpio(GPIO_SPI_CS_BARO); // PD7 MS5611气压计片选
px4_arch_configgpio(GPIO_SPI_CS_HMC); // PC1 HMC5883磁力计片选 Pixhawk上木有HMC啊
px4_arch_configgpio(GPIO_SPI_CS_MPU); // PC2 MPU6000 加速度计/陀螺仪片选
/* De-activate all peripherals,
* required for some peripheral
* state machines
*/
px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
px4_arch_configgpio(GPIO_EXTI_GYRO_DRDY);
px4_arch_configgpio(GPIO_EXTI_MAG_DRDY);
px4_arch_configgpio(GPIO_EXTI_ACCEL_DRDY);
px4_arch_configgpio(GPIO_EXTI_MPU_DRDY);
#endif
#ifdef CONFIG_STM32_SPI2
...
}
这个函数应该熟悉,是文件stm32_spi.c
中SPI1的结构体g_sp1iops
片选成员函数的实现。其作用是根据设备ID(devid)选中一个具体的SPI设备
__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
{
/* SPI select is active low, so write !selected to select the device */
// SPI片选低电平有效。所以写!select就是选中了芯片
switch (devid) {
case PX4_SPIDEV_GYRO:
/* Making sure the other peripherals are not selected */
px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_ACCEL_MAG:
/* Making sure the other peripherals are not selected */
px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_BARO:
...
}
文件px4fmu2_init.c
作用于系统配置和映射所有内存之后,在初始化任何设备之前。执行NSH的架构特定初始化。主要是SPI总线各设备的选择。
__EXPORT int nsh_archinitialize(void)
{
...
/* Configure SPI-based devices */
// 配置基于SPI的设备
spi1 = px4_spibus_initialize(1);
if (!spi1) {
message("[boot] FAILED to initialize SPI port 1\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* Default SPI1 to 1MHz and de-assert the known chip selects. */
// 默认SPI1频率为1MHz,并取消断言已知芯片选择
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);
SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
/* Get the SPI port for the FRAM */
spi2 = px4_spibus_initialize(2);
...
}
它的调用常见的是
/****************************************************************************
* Name: nsh_archinitialize
*
* Description:
* Perform architecture specific initialization for NSH.
*
* CONFIG_NSH_ARCHINIT=y :
* Called from the NSH library
*
* CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
* CONFIG_NSH_ARCHINIT=n :
* Called from board_initialize().
*
****************************************************************************/
#ifdef CONFIG_NSH_LIBRARY
int nsh_archinitialize(void);
#endif
进入mpu6000.cpp
文件。
这个文件主要分为3个部分:MPU6000类的实现(实例化、类成员函数定义)、MPU6000_gyro类的实现(实例化、类成员函数定义)以及一些与shell命令相关的函数定义。
从两个类的定义可以看到,这两个类互为友元类,可以互相访问对方的私有成员函数。
class MPU6000 : public device::CDev
{
public:
MPU6000(device::Device *interface, const char *path_accel, const char *path_gyro, enum Rotation rotation,
int device_type);
...
protected:
...
friend class MPU6000_gyro;
...
private:
...
MPU6000_gyro *_gyro;
...
}
/**
* Helper class implementing the gyro driver node.
*/
class MPU6000_gyro : public device::CDev
{
public:
MPU6000_gyro(MPU6000 *parent, const char *path);
...
protected:
friend class MPU6000;
...
private:
MPU6000 *_parent;
...
}
从代码中可以看出,该传感器的功能实现部分主要是在MPU6000类中实现的,包括传感器连接检测(MPU6000::probe)、设备初始化/加速度计驱动注册(MPU6000::init)、加速度计的I/O通道管理(MPU6000::ioctl)、陀螺仪的I/O通道管理(MPU6000::gyro_ioctl)传感器自检(MPU6000::self_test)、采样频率设置(MPU6000::_set_sample_rate)、数据读取(MPU6000::measure)以及与数据预处理相关的一些操作(数据检验、低通滤波)等等。
而在MPU6000_gyro这个类中,其实只做了一件事情,那就是完成陀螺仪的驱动注册(MPU6000_gyro::init)。虽说MPU6000_gyro类的成员函数中也包含了陀螺仪数据读取(MPU6000_gyro::read)、陀螺仪的I/O通道管理(MPU6000_gyro::ioctl),但是其最终实现都是调用的MPU6000类成员函数
// MPU6000_gyro实例
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
CDev("MPU6000_gyro", path), // 陀螺仪设备端口
_parent(parent), // parent就是一个类MPU6000的对象
...
}
ssize_t
MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen)
{
return _parent->gyro_read(filp, buffer, buflen);// 调用MPU6000::gyro_read
}
int
MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case DEVIOCGDEVICEID: // 获取设备ID
return (int)CDev::ioctl(filp, cmd, arg);
break;
default:
return _parent->gyro_ioctl(filp, cmd, arg);// 调用MPU6000::gyro_ioctl
}
}
总结:主要的功能函数实现还是看MPU6000的类成员函数。
MPU6000加速度计陀螺仪传感器中的加速度计、陀螺仪端口不同。
因为读陀螺仪的数据和其他的数据不是一个端口,所以新建了MPU6000_gyro这个Helper类。MPU6000类内完成加速度计的驱动注册,MPU6000_gyro类内完成陀螺仪的驱动注册。分别注册到fs文件系统后,才能进行file_operation相关的指令:open、read 、write。
以陀螺仪为例介绍一下PX4中如何将一个设备注册到NuttX的文件系统中。
int
MPU6000_gyro::init()
{
int ret;
// do base class init
ret = CDev::init(); // 注册到fs中
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("gyro init failed");
return ret;
}
_gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); //注册节点
return ret;
}
关于这里注册的设备的具体信息,后面会讲到,现在可以简单理解成MPU6000的设备端口
先来看看CDev::init()字符设备初始化的过程
int CDev::init()
{
// base class init first
// 首先初始化基类
int ret = Device::init(); // 注册irq中断
if (ret != OK) {
goto out;
}
// now register the driver
// 现在注册驱动
if (_devname != nullptr) {
ret = register_driver(_devname, &fops, 0666, (void *)this); // 需要关注的是这个_devname对应的设备
if (ret != OK) {
goto out;
}
_registered = true;
}
out:
return ret;
}
看看设备是怎么初始化的:Device::init()
int
Device::init()
{
int ret = OK;
// If assigned an interrupt, connect it
if (_irq) {
/* ensure it's disabled */
up_disable_irq(_irq);
/* register */
// 注册中断
ret = register_interrupt(_irq, this);
if (ret != OK) {
_irq = 0;
}
}
return ret;
}
注册一个中断register_interrupt()
/**
* Register an interrupt to a specific device.
* 向特定设备注册中断。
*
* @param irq The interrupt number to register. 要注册的中断号码
* @param owner The device receiving the interrupt. 接收中断的设备
* @return OK if the interrupt was registered.
*/
static int register_interrupt(int irq, Device *owner){
int ret = -ENOMEM;
// look for a slot where we can register the interrupt
for (unsigned i = 0; i < irq_nentries; i++) {
if (irq_entries[i].irq == 0) {
// great, we could put it here; try attaching it
ret = irq_attach(irq, &interrupt);
if (ret == OK) {
irq_entries[i].irq = irq;
irq_entries[i].owner = owner;
}
break;
}
}
return ret;
}
关于这里为什么用irq(Interrupt Request, 中断请求),能力有限,不得而知。
如果你了解这一块,烦请告知。
注册好中断以后,继续回到字符型设备的初始化函数中来:CDev::init()。现在注册驱动
/****************************************************************************
* Name: register_driver
*
* Description:
* Register a character driver inode the pseudo file system.
* 注册一个字符驱动程序inode到伪文件系统。
*
* Input parameters:
* path - The path to the inode to create
* fops - The file operations structure
* mode - inmode priviledges (not used)
* priv - Private, user data that will be associated with the inode.
*
* Returned Value:
* Zero on success (with the inode point in 'inode'); A negated errno
* value is returned on a failure (all error values returned by
* inode_reserve):
*
* EINVAL - 'path' is invalid for this operation
* EEXIST - An inode already exists at 'path'
* ENOMEM - Failed to allocate in-memory resources for the operation
*
****************************************************************************/
int register_driver(FAR const char *path, FAR const struct file_operations *fops,
mode_t mode, FAR void *priv)
{
FAR struct inode *node;
...
}
笔者无力深究函数内部的实现过程,看函数的注释可以知道这里是注册了一个字符驱动程序inode(索引节点)到文件系统中。而下面这段驱动注册过程
ret = register_driver(_devname, &fops, 0666, (void *)this);
就是将_devname
注册到了文件系统中,这个设备dev每个人可读写。
/*
* chmod指令用数字格式指定权限的改变
* 每个Linux文件具有四种访问权限:可读(r)、可写(w)、可执行(x)和无权限(-)。
* 例如 chmod 777 这里的777分别表示 owner group other
* 模式 数字
* rwx 7
* rw- 6
* r-x 5
* r-- 4
* -wx 3
* -w- 2
* --x 1
* --- 0
*
* 所以代码中常见的666意思是模式为每个人可读和可写
*/
关于inode的介绍,可以参考这篇博客。对索引节点的一个简单理解是,通过它可以找到NuttX操作系统中不同文件(设备),inode中包含了文件除文件名外所有的元信息(文件创建者、创建日期、大小等),Unix/Linux系统内部不使用文件名,而使用inode号码来识别文件。
通过MPU6000_gyro陀螺仪的实例化过程可以推测讨论的陀螺仪设备名称_devname
为path,关于path,继续往下看
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
CDev("MPU6000_gyro", path), // 陀螺仪设备端口
...
通过CDev::init()将字符型设备注册到了文件系统中,然后回到陀螺仪的驱动注册过程MPU6000_gyro::init(),接下来需要将初始化生成的设备节点作为一个设备文件,对应用层开放,可以像访问一个文件一样访问
_gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); //注册节点
其中
#define GYRO_BASE_DEVICE_PATH "/dev/gyro"
#define GYRO0_DEVICE_PATH "/dev/gyro0"
#define GYRO1_DEVICE_PATH "/dev/gyro1"
#define GYRO2_DEVICE_PATH "/dev/gyro2"
注册节点
int CDev::register_class_devname(const char *class_devname)
{
if (class_devname == nullptr) {
return -EINVAL;
}
int class_instance = 0;
int ret = -ENOSPC;
while (class_instance < 4) {
char name[32];
snprintf(name, sizeof(name), "%s%d", class_devname, class_instance);
ret = register_driver(name, &fops, 0666, (void *)this); // 注册驱动
//这里相当于
// ret = register_driver("/dev/gyro", &fops, 0666, (void *)this);
if (ret == OK) { break; }
class_instance++;
}
if (class_instance == 4) {
return ret;
}
return class_instance;
}
到这里/dev/gyro
就可以作为一个文件设备open、read了。而关于调用的Device::init函数进行irq中断注册不是很明白,但是对于PX4中断传感器数据读取的话
驱动层是定时器,最底层听说是中断,然后驱动层的定时器直接去拿已有的数据即可
关于register_class_devname()
这个函数作用应该就是将MPU6000中的陀螺仪这个设备/dev/gyro
作为节点注册到了NuttX系统中。,并且作为一个SPI端口使用。
从下面几幅图可以更加直观的看到整个连接流程
以上这一部分讲的实在是很业余,半猜半理解,对于设备与path的对应尚模棱两可,目的是将陀螺仪注册到文件系统中,后面要加传感器的话,笔者认为可以仿照着来。各取所需吧
就此打住了。
接下来主要分析mpu6000.cpp
的内部逻辑,从程序启动到传感器读取的原始数据处理。进入主函数
int mpu6000_main(int argc, char *argv[])
{
enum MPU6000_BUS busid = MPU6000_BUS_ALL;
int device_type = 6000;
int ch;
bool external = false;
enum Rotation rotation = ROTATION_NONE;
int accel_range = 8;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "T:XISsR:a:")) != EOF) {
switch (ch) {
case 'X':
busid = MPU6000_BUS_I2C_EXTERNAL;
break;
case 'I':
busid = MPU6000_BUS_I2C_INTERNAL;
break;
case 'S':
busid = MPU6000_BUS_SPI_EXTERNAL;
break;
case 's':
busid = MPU6000_BUS_SPI_INTERNAL;
break;
case 'T':
device_type = atoi(optarg);
break;
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
case 'a':
accel_range = atoi(optarg);
break;
default:
mpu6000::usage();
exit(0);
}
}
external = (busid == MPU6000_BUS_I2C_EXTERNAL || busid == MPU6000_BUS_SPI_EXTERNAL);
const char *verb = argv[optind];
/*
* Start/load the driver.
* 开始/加载驱动
*/
if (!strcmp(verb, "start")) {
mpu6000::start(busid, rotation, accel_range, device_type, external);
}
if (!strcmp(verb, "stop")) {
mpu6000::stop(busid);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
mpu6000::test(busid);
}
...
首先是对MPU6000传感器的硬件连接情况的判断,从传感器的启动脚本rc_sensors
可以初见端倪
if ver hwcmp PX4FMU_V2
then
...
# Pixhawk的传感器启动
else
# FMUv2
if mpu6000 start
then
fi
if mpu9250 start
then
fi
if l3gd20 start
then
fi
if lsm303d start
then
fi
fi
fi
对于Pixhawk来说,MPU6000通过内部SPI总线连接。
关于Pixhawk上的MPU6000的总线配置全都是在驱动程序中写死了的,并没有在启动脚本中进行
T:XISsR:a:
的参数定义。
从这里可以看到:
enum MPU6000_BUS busid = MPU6000_BUS_ALL;
int device_type = 6000;
int ch;
bool external = false;
enum Rotation rotation = ROTATION_NONE;
int accel_range = 8;
接下来的command就跟其他的模块一样了,start、stop、test、reset……参数argv[optind]
从启动脚本或者NSH传递到这里
void
start(enum MPU6000_BUS busid, enum Rotation rotation, int range, int device_type, bool external)
{
bool started = false;
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { //遍历
if (busid == MPU6000_BUS_ALL && bus_options[i].dev != NULL) {
// this device is already started
// 设备已经打开了
continue;
}
if (busid != MPU6000_BUS_ALL && bus_options[i].busid != busid) {
// not the one that is asked for
// 不是想要的总线
continue;
}
// 启动特定总线的驱动程序
started |= start_bus(bus_options[i], rotation, range, device_type, external);
}
exit(started ? 0 : 1);
}
其中mpu6000_bus_option
结构体列出了Pixhawk支持的所有总线配置,如下所示
struct mpu6000_bus_option {
enum MPU6000_BUS busid;
const char *accelpath;
const char *gyropath;
MPU6000_constructor interface_constructor;
uint8_t busnum;
MPU6000 *dev;
} bus_options[] = {
#if defined (USE_I2C)
# if defined(PX4_I2C_BUS_ONBOARD)
{ MPU6000_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, &MPU6000_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
# endif
# if defined(PX4_I2C_BUS_EXPANSION)
{ MPU6000_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, &MPU6000_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
# endif
#endif
#ifdef PX4_SPIDEV_MPU
{ MPU6000_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
/*内部SPI,加速度路径,陀螺仪路径,MPU6000的SPI接口,SPI1,null*/
#endif
#if defined(PX4_SPI_BUS_EXT)
{ MPU6000_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, &MPU6000_SPI_interface, PX4_SPI_BUS_EXT, NULL },
#endif
};
然后启动特定总线的驱动程序
bool start_bus(struct mpu6000_bus_option &bus, enum Rotation rotation, int range, int device_type, bool external)
{
int fd = -1;
if (bus.dev != nullptr) {
warnx("%s SPI not available", external ? "External" : "Internal");
return false;
}
device::Device *interface = bus.interface_constructor(bus.busnum, device_type, external);
/*
* 确定设备接口Interface(很重要)
* busid = MPU6000_BUS_SPI_INTERNAL
* accelpath = MPU_DEVICE_PATH_ACCEL(/dev/accel)
* gyropath = MPU_DEVICE_PATH_GYRO(/dev/gyro)
* interface_constructor = MPU6000的内部SPI片选(作为SPI类的加速度计实例)
* busnum = PX4_SPI_BUS_SENSORS(Pixhawk传感器连在SPI1上)
* dev = 是否存在设备连接在此端口上
*/
if (interface == nullptr) {
warnx("no device on bus %u", (unsigned)bus.busid);
return false;
}
if (interface->init() != OK) { // 设备初始化,向特定的设备注册中断请求
delete interface;
warnx("no device on bus %u", (unsigned)bus.busid);
return false;
}
bus.dev = new MPU6000(interface, bus.accelpath, bus.gyropath, rotation, device_type); // 新建MPU6000类的实例
if (bus.dev == nullptr) {
delete interface;
return false;
}
if (OK != bus.dev->init()) { // MPU6000::init()
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(bus.accelpath, O_RDONLY);
if (fd < 0) {
goto fail;
}
// 注意ioctl:关于传感器轮询模式的配置,自动轮询/手动轮询;截止频率
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
if (ioctl(fd, ACCELIOCSRANGE, range) < 0) {
goto fail;
}
close(fd);
return true;
fail:
if (fd >= 0) {
close(fd);
}
if (bus.dev != nullptr) {
delete(bus.dev);
bus.dev = nullptr;
}
return false;
}
这个函数干的事情多了,确定MPU6000的最终设备ID,新建MU6000实例,主要是它调用了MPU6000::init()(前文中已经说明过其实现过程)。而MPU6000::init()是整个传感器功能的实现,函数中先将MPU6000注册到文件系统中,包括加速度计、陀螺仪两个设备,然后进行数据测量measure()
。
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
// 传感器作为一个文件设备,操作步骤
// open -> ioctl -> read -> close
void
test(enum MPU6000_BUS busid)
{
struct mpu6000_bus_option &bus = find_bus(busid);
accel_report a_report;
gyro_report g_report;
ssize_t sz;
/* get the driver */
int fd = open(bus.accelpath, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'mpu6000 start')", bus.accelpath);
}
/* get the driver */
int fd_gyro = open(bus.gyropath, O_RDONLY);
if (fd_gyro < 0) {
err(1, "%s open failed", bus.gyropath);
}
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) {
err(1, "reset to manual polling");
}
/* do a simple demand read */
sz = read(fd, &a_report, sizeof(a_report));
if (sz != sizeof(a_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(a_report));
err(1, "immediate acc read failed");
}
warnx("single read");
warnx("time: %lld", a_report.timestamp);
warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x);
warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y);
warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z);
warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
(double)(a_report.range_m_s2 / MPU6000_ONE_G));
/* do a simple demand read */
sz = read(fd_gyro, &g_report, sizeof(g_report));
if (sz != sizeof(g_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(g_report));
err(1, "immediate gyro read failed");
}
warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature);
warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
/* reset to default polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "reset to default polling");
}
close(fd);
close(fd_gyro);
/* XXX add poll-rate tests here too */
reset(busid);
errx(0, "PASS");
}
从这个函数可以看出,PX4中的传感器数据读取是可以按照基本的文件操作方法实现的,
open → ioctl → read → close
印证了文章最开始的想法。
但是这样错太粗糙了,还是得进行数据预处理的。
这个函数中是从传感器读数并进行数据处理并发布数据的过程。直接关系到传感器的最终读数。
数据读取过程由下面的代码实现
// sensor transfer at high clock speed
// 高速读取
if (sizeof(mpu_report) != _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_HIGH_BUS_SPEED),
(uint8_t *)&mpu_report,
sizeof(mpu_report))) {
return -EIO;
}
check_registers(); // 寄存器数据检查
显然,使用的是read
,将mpu6000作为文件进行读操作。
接下来就是数据处理了
/*
* Convert from big to little endian
* 从大端到小端
*
* Little-Endian就是低位字节排放在内存的低地址端,高位字节排放在内存的高地址端。
* Big-Endian就是高位字节排放在内存的低地址端,低位字节排放在内存的高地址端。
*/
report.accel_x = int16_t_from_bytes(mpu_report.accel_x); // 将测得的值mpu_report传递给将发布的值report
report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
report.temp = int16_t_from_bytes(mpu_report.temp);
report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
mpu_report为传感器测得值,report为最终采集到的值(需进一步处理)。
int16_t int16_t_from_bytes(uint8_t bytes[])
{
union {
uint8_t b[2];
int16_t w;
} u;
u.b[1] = bytes[0];
u.b[0] = bytes[1];
return u.w;
}
对于一个16位传感器MPU6000,其数据存储寄存器分为高八位、低八位,各占一个字节。
SPI协议的话,数据时一位一位地读取,因此以大端模式保存到2个含8个元素的数组uint8_t b[2]
中,但最终要处理的是一个16位的无符号整型数据int16_t w
,所以要进行大小端数据转换。
对于Pixhawk来说,MPU6000是放置在飞控板底面的,也就是绕其自身Y轴旋转了180°。
而对于PX4固件来说,其使用的是NED坐标系,要完成软硬件的匹配,在驱动层采用了交换XY轴的方法。
/*
* Swap axes and negate y
*
* 交换xy轴读数并将新的y轴读取取负
*
* 理由是 正放的话,MPU6K y向前,x向右, z向上。 但是
* Pixhawk 中,传感器是倒置的, x向前, y向右,z向下。
*/
int16_t accel_xt = report.accel_y;
int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
int16_t gyro_xt = report.gyro_y;
int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
/*
* Apply the swap
*/
report.accel_x = accel_xt;
report.accel_y = accel_yt;
report.gyro_x = gyro_xt;
report.gyro_y = gyro_yt;
接下来是一连串的数据处理过程:二阶低通滤波、设置分辨率/缩放因子、积分环节、自定义旋转,我们只需要关心最后的发布即可。
if (accel_notify && !(_pub_blocked)) {
/* log the time of this report */
perf_begin(_controller_latency_perf);
/* publish it */
//////////////////////// 发布加速度计主题 //////////////////////
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (gyro_notify && !(_pub_blocked)) {
/* publish it */
//////////////////////// 发布陀螺仪主题 //////////////////////
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
调用此函数的话,启动自动测量模式。详情请查看MPU6000::ioctl()
函数
void
MPU6000::start()
{
/* make sure we are stopped first */
uint32_t last_call_interval = _call_interval;
stop();
_call_interval = last_call_interval;
/* discard any stale data in the buffers */
// 清空缓冲区
_accel_reports->flush();
_gyro_reports->flush();
if (!is_i2c()) {
/* start polling at the specified rate */
// 以指定的速率开始轮询
hrt_call_every(&_call,
1000,
_call_interval - MPU6000_TIMER_REDUCTION,
(hrt_callout)&MPU6000::measure_trampoline, this); // 定时器
} else {
// 与I2C相关的所有都不用管,Pixhawk的MPU6000接在SPI总线上
#ifdef USE_I2C
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&MPU6000::cycle_trampoline, this, 1);
#endif
}
}
接下来就是循环测量更新了
void MPU6000::cycle() //循环
{
int ret = measure();
if (ret != OK) {
/* issue a reset command to the sensor */
reset();
start();
return;
}
if (_call_interval != 0) {
work_queue(HPWORK,
&_work,
(worker_t)&MPU6000::cycle_trampoline,
this,
USEC2TICK(_call_interval - MPU6000_TIMER_REDUCTION));
}
}
#endif
void MPU6000::measure_trampoline(void *arg)
{
MPU6000 *dev = reinterpret_cast(arg);
/* make another measurement */
dev->measure(); // 数据测量
}
最后的话,提一下文件中对寄存器读数有效性检查的处理
首先,对于MPU6000来说,其内部关键的寄存器列表如下
// 使用的寄存器列表
const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID,
MPUREG_PWR_MGMT_1, /* 电源 */
MPUREG_USER_CTRL,
MPUREG_SMPLRT_DIV, /* 频率 */
MPUREG_CONFIG,
MPUREG_GYRO_CONFIG, /* 加计量程 */
MPUREG_ACCEL_CONFIG, /* 陀螺量程 */
MPUREG_INT_ENABLE,
MPUREG_INT_PIN_CFG,
MPUREG_ICM_UNDOC1
};
检查寄存器
void MPU6000::check_registers(void)
{
uint8_t v;
// the MPUREG_ICM_UNDOC1 is specific to the ICM20608 (and undocumented)
// 不是 ICM20608
if (_checked_registers[_checked_next] == MPUREG_ICM_UNDOC1 && !is_icm_device()) {
_checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS;
}
if ((v = read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
_checked_values[_checked_next]) { // 从寄存器读取到的值和写入的值不同
// 如果我们得到错误的值,那么我们知道SPI总线或传感器问题很严重 。
// 我们将_register_wait设置为20,然后等连续看到20个良好的值。
// 再次认为传感器健康,
perf_count(_bad_registers);
/*
try to fix the bad register value. We only try to
fix one per loop to prevent a bad sensor hogging the
bus.
*/
if (_register_wait == 0 || _checked_next == 0) {
// if the product_id is wrong then reset the
// sensor completely
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); // 0x80
// after doing a reset we need to wait a long
// time before we do any other register writes
// or we will end up with the mpu6000 in a
// bizarre state where it has all correct
// register values but large offsets on the
// accel axes
_reset_wait = hrt_absolute_time() + 10000;
_checked_next = 0;
} else {
write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); // 向寄存器写入检验后的值
_reset_wait = hrt_absolute_time() + 3000;
}
_register_wait = 20;
}
_checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS;
}
写一个寄存器,更新_checked_values
/**
* Write a register in the MPU6000, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void MPU6000::write_checked_reg(unsigned reg, uint8_t value)
{
write_reg(reg, value); // 写寄存器
for (uint8_t i = 0; i < MPU6000_NUM_CHECKED_REGISTERS; i++) {
if (reg == _checked_registers[i]) { // 寄存器列表中有这个寄存器
_checked_values[i] = value; // 将写入的寄存器值赋给_checked_values[i]
}
}
}
From 吴神
说mpu6000为什么要有个gyro class
两个原因
第一,有的传感器分两三块类型的数据,比如陀螺和加速度,但是这多种数据的到达时间是不一样的,就是DRDY引脚的高电平响应,所以要分开来采样
第二,传感器有不同的数据类型也决定了,有的时候有些部分是坏的,不能用。比如说LSM303D这个传感器,加速度计就容易坏掉,这种情况下为了不影响磁力计的输出,这个cpp也是把两个分开来的
过程有些凌乱,不应该。
笔者觉得一个真的懂这些的人是能够把问题的本质抽象出来的,可以以简单的流程图说明整个过程的,能让旁人一看就懂的,革命尚未成功。
接下来就准备自定义添加一枚传感器了,同志仍需努力。
参考
虾米一代的博客
summer的培训资料
By Fantasy