適用于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替換的地方。

總結

以上是生活随笔為你收集整理的PX4原生固件SPI驱动动编写与IMU传感器替换的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。