適用於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 class
    CDev(name, devname, irq),
    // public
    // protected
    locking_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底層驅動裡面。

二 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
then
    echo "ERROR [init] Estimator INAV deprecated. Using EKF2"
    param set SYS_MC_EST_GROUP 2
    param 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 start
    then
        local_position_estimator start
    else
        echo "ERROR [init] Estimator LPE not available. Using EKF2"
        param set SYS_MC_EST_GROUP 2
        param save
    fi
fi

# EKF
#if param compare SYS_MC_EST_GROUP 2
#then
#    ekf2 start
#fi
#---------------------------------------
#xsens_mti3 start

mc_att_control start

mc_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替換的地方。