【背景描述】

AutoQuad是德國的一款老牌開源飛控（硬件閉源），其旨在提供穩定、動態飛行和自動駕駛功能的飛控控制器。

由于AutoQuad硬件閉源的特性，國內的玩家很少，但AutoQuad 的ukf算法“獨步天下”，絕對是一絕，我對其垂涎已久。15年時我自己做出了Autoquad的M4版本硬件（基于stm32f405rgt6），可以運行官方源碼。

17年時我將Autoquad移植到mdk環境下并且將其rtos替換為RT-Thread。后續玩這個玩了蠻久時間，這個版本的AutoQuad有一個問題：由于UKF算法占用了很多cpu資源使得整個系統cpu占用率太高，再者就是片內ram資源捉襟見肘。

對于這個版本的AutoQuad目前有挺多模友想繼續深入的開發，比如網名為“我的世界觀”的網友想將L1自適應控制算法加入到其中，但這個L1自適應算法也是極耗費cpu資源的。在這個背景下我開始著手AutoQuad在imx-rt1052上的實現，以期留出足夠的資源給大家來給模友們深入開發，同時我也借機熟悉下RT-Thread的3.x版本。

另外參加這個作品征集活動只是這個項目的開始，我后續會將成果放在github，歡迎大家一起來加入這個項目，繼續延續AutoQuad的傳奇。

【所用物料】

主控：

iMX-RT1052DVL6B

傳感器：

ICM20602、SPL06、HMC5983

編譯環境:

WIN10、MDK5.25

RT-Thread 版本：

3.0.4

實物圖：

硬件板子目前基于野火1052 mini開發板，傳感器是從馬家買的現成模塊，采用飛線的形式固定在開發板上（后期會重新設計一款小的適合飛控的板子）

主控+傳感器

全部的連接都使用飛線（感謝火哥不殺之恩）

為了上蓋能蓋的下，將底板上的usb座和網絡接口座去掉了（感謝火哥不殺之恩）

勉強能扣好

【硬件設計】

系統框圖

硬件設計解析

A、傳感器部分：

各個傳感器接到IMX-RT1052的SPI3上，進行分時操作。

B、SBUS輸入

由于SBUS信號是一個反向電平的串口信號，這里使用一個三級管搭了一個簡單的反向電路，從而將信號正確到接入到處理器的串口輸入端。

C、PWM 信號輸出

用于控制ESC的PWM信號，使用主控上PWM1和PWM2中的AB通道，這里3的意思是使用submode3。

D、GPS

GPS模塊是一個獨立的單元，通過串口接到主控的串口5上，上圖中的原理圖是我早期時候設計的基于Ublox M8N的GPS模塊，這里剛好用到。

【軟件設計】

軟件設計流程

整個AutoQuad源碼是一個較大的系統，我這里抽絲剝繭，將其中的主脈絡流程貼出來：

關鍵代碼解析

此版和之前在stm32f405上的版本最大的區別在于加速度+陀螺儀傳感器、磁力計、高度計的數據讀取上。原版直接使用spi進行驅動，這個版本我使用了RT-Thread的SPI設備驅動框架來進行數據讀取。

這里將加速度傳感器&陀螺儀驅動源碼列出來，進行一個簡單解析：

1、將總線設備掛到總線上（配置CS引腳）

rt1050_spi_bus_attach_device("spi3", "spi32", 64);

此段代碼表示將icm20602作為spi3上的第三個設備和spi總線進行關聯，并使用 GPIO_AD_B1_05 作為其cs引腳（其中64代表 GPIO_AD_B1_05 ，即icm20602的cs引腳是 GPIO_AD_B1_05 ）。

2、配置SPI總線相關參數

struct rt_spi_configuration cfg; cfg.data_width = 8; cfg.mode = RT_SPI_MODE_0 | RT_SPI_MSB; /* SPI Compatible: Mode 0 and Mode 3 */ cfg.max_hz = MPU6000_SPI_BAUD; /* 1M */ rt_spi_configure(spi_acc_gyro_device->rt_spi_device, &cfg);

3、對總線的互斥操作

為保證多個設備對spi3的互斥使用，需要對icm20602加入互斥鎖操作：

static void sensor_lock(struct spi_acc_gyro_device * sensor_device) { rt_mutex_take(sensor_device->lock, RT_WAITING_FOREVER); } static void sensor_unlock(struct spi_acc_gyro_device * sensor_device) { rt_mutex_release(sensor_device->lock); }

4、寄存器的讀寫操作

static void icm20602_write_reg(uint8_t reg, uint8_t value) { uint8_t send_buffer[2]; send_buffer[0] = reg&0x7f; send_buffer[1] = value; rt_spi_send(spi_acc_gyro_device->rt_spi_device, send_buffer, 2); } static uint8_t icm20602_read_reg(uint8_t reg) { uint8_t rxBuf[2]; uint8_t txBuf[2]; txBuf[0] = reg|0x80; rt_spi_send_then_recv(spi_acc_gyro_device->rt_spi_device, txBuf, 1, rxBuf, 2); return rxBuf[0]; } static uint8_t icm20602_read_buffer(uint8_t reg, uint8_t *buffer, uint16_t len) { uint8_t txBuf[2]; uint8_t rxBuf[20]; sensor_lock(spi_acc_gyro_device); txBuf[0] = reg|0x80; rt_spi_send_then_recv(spi_acc_gyro_device->rt_spi_device, txBuf, 1, rxBuf, len); rt_memcpy(buffer, rxBuf, len); sensor_unlock(spi_acc_gyro_device); return 0; }

5、讀取加速度和陀螺儀數據

static uint8_t icm20602_get_accel(int16_t *accel, int16_t *temp) { uint8_t buf[10]; icm20602_read_buffer(ICM20_ACCEL_XOUT_H,buf,8); accel[0] = ((int16_t)buf[0]<<8) + buf[1]; ? ?accel[1] = ((int16_t)buf[2]<<8) + buf[3]; ? ?accel[2] = ((int16_t)buf[4]<<8) + buf[5]; ? ?*temp ? ? = ((int16_t)buf[6]<<8) + buf[7]; ? ?//ICM_TRACE("acc0=%d, acc1=%d, acc2=%d ",accel[0],accel[1],accel[2]); ? ?return 0; } static uint8_t icm20602_get_gyro(int16_t *gyro) { ? ?uint8_t buf[8]; ? ?icm20602_read_buffer(ICM20_GYRO_XOUT_H,buf,8); ? ?gyro[0] = (buf[0]<<8) + buf[1]; ? ?gyro[1] = (buf[2]<<8) + buf[3]; ? ?gyro[2] = (buf[4]<<8) + buf[5]; ? ?// ICM_TRACE("gyro0=%d, gyro1=%d, gyro2=%d ",gyro[0],gyro[1],gyro[2]); ? ?return 0; }

因為 icm20602_read_buffer 函數內部已經加入了互斥，所以讀取時不再需要互斥操作。

RT-Thread 使用情況

1、動態內存管理；

2、GPIO設備驅動架構；

3、RTC設備驅動架構；

4、SPI設備驅動架構；

5、SDIO設備驅動架構；

6、串口設備驅動架構；

7、MSH命令行；

8、FatFs文件系統。