Pixhawk原生固件PX4之MPU6000驱动分析

欢迎交流~ 个人 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即可，思路是正确的，但是需要一些前提条件的：

1. 驱动注册。只有将设备注册到系统中才能进行文件操作的，而且怎么保证你打开的设备是你想要打开的？MPU6000的加速度计、陀螺仪算是两个设备了
2. 端口配置。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)

• 以及SPI总线的宏定义

#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不知道用上没有。

• 以及SPI1总线上的设备枚举

#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

与SPI操作相关的函数

spi.h

如同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

文件stm32_spi.c中是spi协议的函数实现。

首先以SPI1为例，g_sp1iops是一个spi_ops_s结构体，可以类似的理解为SPI这个类的实例，包含了所有的成员.select、setmode等，并且对应的完成了功能函数的实现，如spi_setfrequency、spi_setmode等。stm32_spi1select的实现在后面的文件中会介绍。

#ifdef CONFIG_STM32_SPI1static 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_portinitialize

/* 将所选的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   */...}

• up_spiinitialize

/* 初始化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

文件px4fmu_spi.c中是一些Pixhawk飞控板特定的SPI函数

• stm32_spiinitialize

/* 为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_spi1select

这个函数应该熟悉，是文件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

文件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_LIBRARYint nsh_archinitialize(void);#endif

MPU6000驱动分析

进入mpu6000.cpp文件。

这个文件主要分为3个部分：MPU6000类的实现(实例化、类成员函数定义)、MPU6000_gyro类的实现(实例化、类成员函数定义)以及一些与shell命令相关的函数定义。

MPU6000类和MPU6000_gyro类

从两个类的定义可以看到，这两个类互为友元类，可以互相访问对方的私有成员函数。

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_tMPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen){    return _parent->gyro_read(filp, buffer, buflen);// 调用MPU6000::gyro_read}intMPU6000_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的文件系统中。

intMPU6000_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()

intDevice::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端口使用。

从下面几幅图可以更加直观的看到整个连接流程

• 注册类设备名(直译)

• 注册驱动

• SPI节点操作

以上这一部分讲的实在是很业余，半猜半理解，对于设备与path的对应尚模棱两可，目的是将陀螺仪注册到文件系统中，后面要加传感器的话，笔者认为可以仿照着来。各取所需吧

就此打住了。

MPU6000的启动过程

接下来主要分析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_V2then...# Pixhawk的传感器启动    else        # FMUv2        if mpu6000 start        then        fi        if mpu9250 start        then        fi        if l3gd20 start        then        fi        if lsm303d start        then        fi    fifi

对于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传递到这里

• start → 打开驱动

voidstart(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()。

• test → 驱动测试

/** * 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 -> closevoidtest(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

印证了文章最开始的想法。

但是这样错太粗糙了，还是得进行数据预处理的。

数据处理过程

数据测量 MPU6000::measure()

这个函数中是从传感器读数并进行数据处理并发布数据的过程。直接关系到传感器的最终读数。

read

数据读取过程由下面的代码实现

// 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传递给将发布的值reportreport.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，所以要进行大小端数据转换。

交换XY轴

对于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::start()

调用此函数的话，启动自动测量模式。详情请查看MPU6000::ioctl()函数

voidMPU6000::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));    }}#endifvoid MPU6000::measure_trampoline(void *arg){    MPU6000 *dev = reinterpret_cast<MPU6000 *>(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]         }    }}

Tips

From 吴神

说mpu6000为什么要有个gyro class

两个原因

第一，有的传感器分两三块类型的数据，比如陀螺和加速度，但是这多种数据的到达时间是不一样的，就是DRDY引脚的高电平响应，所以要分开来采样

第二，传感器有不同的数据类型也决定了，有的时候有些部分是坏的，不能用。比如说LSM303D这个传感器，加速度计就容易坏掉，这种情况下为了不影响磁力计的输出，这个cpp也是把两个分开来的

---

过程有些凌乱，不应该。

笔者觉得一个真的懂这些的人是能够把问题的本质抽象出来的，可以以简单的流程图说明整个过程的，能让旁人一看就懂的，革命尚未成功。

接下来就准备自定义添加一枚传感器了，同志仍需努力。

参考

• 虾米一代的博客

• summer的培训资料

---

                                          By Fantasy