PX4原生固件SPI驱动动编写与IMU传感器替换

适用于PX4原生固件
核心目标：完成XSENS的MTI3，IMU替换。MTI3是一款航姿参考系统，可以独立的输出四元数，加速度，磁力计等，角速度等航姿信息。里面有完整的卡尔曼滤波，可以替换飞控本身里面的姿态估计部分。因为PX4里面所用的传感器器件都是消费级的元器件，所以MTI3这样的工业级的IMU替换还是非常有价值的。

一 PX4：SPI硬件介绍

PIXHAWK里面有3路SPI的硬件接口，分别是：

IMU的一路（通过片选信号来支持磁力计，陀螺仪，加速度计这几个SPI传感器）
铁电存储器一路（存储飞控参数信息）

外置SPI接口一路，可以外接SPI传感器的

                   外置SPI接口线路图

二 PX4：SPI驱动介绍

pixhawk内部的很多传感器都是用SPI进行通信的，所有SPI接口传感器都是基于继承了device::SPI这个SPI基类来实现的SPI传感器驱动的编写。比如：
Src/Drivers/Hmc5883 磁力计，Src/Drivers/Mpu9250 陀螺仪等等都是基于SPI总线。
里面的构造函数，init，read，write，ioctl几个虚函数，在本类里面重写即可。这里我们可以去看一下MPU9250，Hmc5883的SPI驱动的写法
尤其这几个函数的写法。比如MPU9250的：

比如5883的：

这个虚函数的重写内容都是不一样的，具体的写法规则要更具具体的硬件手册来。所以驱动的编写硬件很重要我们看下SPI的硬件相关的内容。

我们可以看到我们要操作的这个外置IMU的硬件接口是SPI4和一个SPI的Drdy接口预留。
这是我们可以初步可以分析到的内容，软件上就是继承SPI基类，硬件上就是SPI4外置SPI接口。
在nuttx系统层面上我们也可以看到一些东西：

在nuttx系统的dev文件夹下面可以看到这些设备的文件，也就是说我们自己添加的设备文件也可以在这里看到。nuttx操作系统和linux类似，一切皆文件。
以上是我们可以直观感受到的SPI驱动相关的硬件和软件部分，我们先有直观的认识。我们可以仿造5883的写法写好我们自己的SPI驱动的函框架
那么我们要详细的看下这个SPI基类了，在src\drivers\device\spi.cpp中我们可以看到这个SPI基类：

SPI::SPI(const char *name,const char *devname,int bus,enum spi_dev_e device,enum spi_mode_e mode,uint32_t frequency,int irq) :// base classCDev(name, devname, irq),// public// protectedlocking_mode(LOCK_PREEMPTION),// private_device(device),_mode(mode),_frequency(frequency),_dev(nullptr),_bus(bus)
{// fill in _device_id fields for a SPI device_device_id.devid_s.bus_type = DeviceBusType_SPI;_device_id.devid_s.bus = bus;_device_id.devid_s.address = (uint8_t)device;// devtype needs to be filled in by the driver_device_id.devid_s.devtype = 0;
}

可以看到这些函数的传入函数name,设备名，驱动类型的枚举，SPI模式的枚举，SPI时钟频率，中断。我们在追踪SPI类，会发现他继承的VDev类。
先不管这些我们来分析如果我们要新建一个我们自己的SPI类应该怎么办，那么我们看下MPU5883的看他怎么写的：
```
HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) :SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */)
{_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
}
```
这是HMC5883_SPI的构造函数，我们可以看到要传入bus，device，device_type参数，其中注意到
SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 1000 1000 / will be rounded to 10.4 MHz /)
SPIDEV_MODE3和11 1000 1000指定了SPI的时钟模式和SPI的传输速度（传输速度和传感器硬件有关系，传感器手册有，按照这个来）
这是传递给SPI这个基类的参数，做了一部分SPI的初始化的工作。但是这个bus和device我们还不知道，继续跟踪下。在HMC5883_SPI_interface(int bus)这个函数里面实例化了HMC5883_SPI这个类
return new HMC5883_SPI(bus, (spi_dev_e)PX4_SPIDEV_HMC);
传递了bus，device

第二个参数选择了SPI总线的片选信号线PX4_SPIDEV_HMC就是片选信号选择，我们知道这一版pixhawk的IMU传感器的通信都是基于SPI的，磁力计，陀螺仪都是SPI总线，我们选择了哪一个SPI接口，就要相应的片选使能，使能以后就可以读取相应的传感器参数。

这是V2这个硬件片选定义的地方

那么第一个参数是选择对应的SPI总线。

注PIXHAWk有三路SPI总线接口，一路给铁电存储器，一路给内置IMU，一路给了外置的SPI。我们最后是要实现外置SPI的操作，但是先分析下这个内置的IMU的SPI总线。
bus是

bool
start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
{if (bus.dev != nullptr) {errx(1, "bus option already started");}device::Device *interface = bus.interface_constructor(bus.busnum);if (interface->init() != OK) {delete interface;warnx("no device on bus %u (type: %u)", (unsigned)bus.busnum, (unsigned)bus.busid);return false;}bus.dev = new HMC5883(interface, bus.devpath, rotation);if (bus.dev != nullptr && OK != bus.dev->init()) {delete bus.dev;bus.dev = NULL;return false;}int fd = open(bus.devpath, O_RDONLY);if (fd < 0) {return false;}if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {close(fd);errx(1, "Failed to setup poll rate");}close(fd);return true;
}

bus.interface_constructor(bus.busnum);选择了SPI的传感器总线。这个5883选择的是PX4_SPI_BUS_SENSORS这个内置IMU SPI传感器总线，因为在硬件上IMU的各个传感器都是用的一路SPI接口，只是他们的片选信号线不同，片选信号线我们在前面看见了参数说明，

总结下就创建SPI类的入口，还有各种参数的意义
SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 1000 1000 / will be rounded to 10.4 MHz /)核心就是在填充整个参数，知道各个参数的含义很重要
"HMC5883_SPI"是名字，bus是那个SPI总线，device是指定相应的片选信号， SPIDEV_MODE3是你的SPI时钟模式，11 1000 1000是你的SPI的传感器读取速度。这些参数都对应了硬件的配置和物理的硬件接口要会分析硬件电路图。
其实我们在SPI里面跟踪也会发现有个transfer函数，这个函数就是发送和读取的函数了，里面有：
```
SPI_SETFREQUENCY(_dev, _frequency);SPI_SETMODE(_dev, _mode);SPI_SETBITS(_dev, 8);SPI_SELECT(_dev, _device, true);/* do the transfer */SPI_EXCHANGE(_dev, send, recv, len);/* and clean up */SPI_SELECT(_dev, _device, false);
```
这几个函数实际上在调用Firmware/Nuttx/nuttx/arch/arm/src/stm32/stm32_spi.c系统的底层SPI库
最开始的核心是SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 1000 1000 / will be rounded to 10.4 MHz /)这几个参数的理解，要更具硬件电路图来理解

1. 用start_bus函数实例化HMC5883_SPI类

2.用start_bus 实例化HMC5883类

3.把HMC5883_SPI类里面ioctl,write,read,init几个虚函数函数的重写

4.HMC5883类里面开启工作队列work_queue或者定时回调函数来读取传感器的值，然后通过ourb把数据发送出去。

5. 重点分析下SPI基类里面的函数 SPI_SETMODE，SPI_SELECT，SPI_EXCHANGE，SPI_SELECT几个函数的理解在Firmware/Nuttx/nuttx/arch/arm/src/stm32/stm32_spi.c底层驱动里面。

还有这篇文章也是不错的，详细描述的底层的关系：
http://blog.csdn.net/czyv587/article/details/53817154
可以看看。

二 XSENS_MTI3航姿参考系统的特性和替换

MTI3是XSENS公司推出的航姿参考系统，其中IMU单元直接输出四元数，这些四元数是经过这个硬件的IMU模块解算好的（内置了卡尔曼滤波），可以直接替换飞机的姿态检测部分。其中内置的陀螺仪，磁力计，加速度计都是工业级的，抗干扰和稳定性都优于pixhawk自带的IMU，9250等陀螺仪都是消费级的传感器，所以这方面的替换很有必要。我们在这里替换了PX4系统里面的姿态检测部分。主要工作就是根据MTI3这个传感器的SPI使用手册，来实现读写操作，原始数据解析工作和原有的ourb消息的替换工作。
MTI3传感器注意要点
1 接口外设选择为SPI的
2 DRDY数据就绪从MTI3硬件板子上引出来，接到飞控上，作为数据的就绪选择端
3 用专门的MTI3传感器配置软件，把传感器配置为四元数输出
4 用逻辑分析仪来调试SPI的驱动

1 MTI3选择配置为四元数，加速度计，磁罗盘输出
选择开关配置：

我们先配置好为USB和电脑通信，来配置模块为SPI输出和四元数，加速度，角速度，磁力计输出：

按照上面的配置参数来，我们点击发送以后我们可以在预览界面里面看见各种输出，包括四元数，加速度和磁力计的。
接下来我们把拨码开关拨到SPI输出模式，接好线给飞控的外置SPI接口。开始写这个却动程序。

还有比较重要的硬件改造

这个白色的线的接口是飞控的PC14号接口，也是系统预留的DRDY接口。这个引脚的作用是告诉SPI主设备，从机SPI的一包数据已经准备好了，可以读取了。这个硬件改造比较重要，否则会造成数据读取错误。

到这里所有的硬件配置已经完毕，就是结合飞控写SPI驱动了。
整个SPI的驱动已提供好了，驱动调试，逻辑分析仪是少不了的。

上面是SPI驱动部分源码，不定期更新代码修复bug，请关注！
xsens_mti3_spi.cpp是SPI的读写类实现了读写操作
xsens_mti3.cpp是主函数，里面实例化了xsens_mti3_spi类来实现传感器的读写，用了hrt_call_every定时器来循环读取传感器参数，通过UORB发送出去。
mtinterface.cpp是读取缓冲区和数据解析的接口函数，MTI3是用的X_BUS协议，数据读取采用了缓冲区，xbusmessage.cpp，xbusparser.cpp
xbusutility.cpp都是数据解析函数

int
XSENS_MTI3::collect()
{//warnx("collect()");uint16_t notificationMessageSize = 0;uint16_t measurementMessageSize = 0;readPipeStatus(&notificationMessageSize, &measurementMessageSize);uint16_t size = 0;uint8_t pipe = 0;memset(&xbusMessagebuf,0,sizeof(xbusMessagebuf));memset(&rebuffer[2],0,sizeof(rebuffer) - 2);if (notificationMessageSize){size = notificationMessageSize;pipe = 0x05;}else if (measurementMessageSize){size = measurementMessageSize;pipe = 0x06;}else{return -1;}_interface->read(pipe,&rebuffer[2],size);XbusParser_parseBuffer(m_xbusParser, rebuffer, 2 + size);if(getXbusMessage(&xbusMessagebuf)){handleXbusMessage(&xbusMessagebuf);}return OK;
}

XbusParser_parseBuffer原始数据放入缓冲区
getXbusMessage(&xbusMessagebuf)得到消息包数据
handleXbusMessage解析数据
在解析函数handleXbusMessage中把解析到的数据通过uorb发送出去
同时有个DRDY引脚检测判断什么时候应该读取数据了（ mti3_drdy_status = MTI3_DRDY）

void
XSENS_MTI3::cycle()
{mti3_drdy_status = MTI3_DRDY;if(mti3_drdy_status){collect();}else{;}
}

说到UORB发送数据，我们到底要替换什么数据。最新版的PX4构架的代码已经用EKF2来整合了姿态估计和位置估计的代码，姿态估计和位置估计是一体的。所以我们还是采用了以往的
modules/attitude_estimator_q
modules/local_position_estimator
我们还是采用的LPE来单独的位置估计和attitude_estimator_q开进行单独的状态估计，这里是要修改编译脚本和启动脚本，就是是修改nuttx_px4fmu-v2_default.cmake和rc.mc_apps
具体rc.mc_apps修改如下：

#!nsh
#
# Standard apps for multirotors:
# att & pos estimator, att & pos control.
##---------------------------------------
# Estimator group selction
#
# INAV (deprecated)
if param compare SYS_MC_EST_GROUP 0
thenecho "ERROR [init] Estimator INAV deprecated. Using EKF2"param set SYS_MC_EST_GROUP 2param save
fi# LPE
if param compare SYS_MC_EST_GROUP 2
then# Try to start LPE. If it fails, start EKF2 as a default# Unfortunately we do not build it on px4fmu-v2 due to a limited flash.if xsens_mti3 start#if attitude_estimator_q startthenlocal_position_estimator startelseecho "ERROR [init] Estimator LPE not available. Using EKF2"param set SYS_MC_EST_GROUP 2param savefi
fi# EKF
#if param compare SYS_MC_EST_GROUP 2
#then
#    ekf2 start
#fi
#---------------------------------------
#xsens_mti3 startmc_att_control startmc_pos_control start#
# Start Land Detector
#
land_detector start multicopter

我们强制启动了xsens_mti3 start和local_position_estimator start，以往的attitude_estimator_q start也屏蔽掉，因为我们的姿态估计用的是 xsens_mti3 start。那么实际上姿态估计也很简单主要是发布了两个消息主题：
orb_publish(ORB_ID(vehicle_attitude),_att_pub, &att);
orb_publish(ORB_ID(control_state),_ctrl_state_pub,&ctrl_state);
姿态和控制状态。因为这个MIT3是不需要校准的，所以我们没有去管校准的问题。以上就是主要IMU替换的地方。