【背景描述】

AutoQuad是德國(guó)的一款老牌開(kāi)源飛控（硬件閉源），其旨在提供穩(wěn)定、動(dòng)態(tài)飛行和自動(dòng)駕駛功能的飛控控制器。

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

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

對(duì)于這個(gè)版本的AutoQuad目前有挺多模友想繼續(xù)深入的開(kāi)發(fā)，比如網(wǎng)名為“我的世界觀”的網(wǎng)友想將L1自適應(yīng)控制算法加入到其中，但這個(gè)L1自適應(yīng)算法也是極耗費(fèi)cpu資源的。在這個(gè)背景下我開(kāi)始著手AutoQuad在imx-rt1052上的實(shí)現(xiàn)，以期留出足夠的資源給大家來(lái)給模友們深入開(kāi)發(fā)，同時(shí)我也借機(jī)熟悉下RT-Thread的3.x版本。

另外參加這個(gè)作品征集活動(dòng)只是這個(gè)項(xiàng)目的開(kāi)始，我后續(xù)會(huì)將成果放在github，歡迎大家一起來(lái)加入這個(gè)項(xiàng)目，繼續(xù)延續(xù)AutoQuad的傳奇。

【所用物料】

主控：

iMX-RT1052DVL6B

傳感器：

ICM20602、SPL06、HMC5983

編譯環(huán)境:

WIN10、MDK5.25

RT-Thread 版本：

3.0.4

實(shí)物圖：

硬件板子目前基于野火1052 mini開(kāi)發(fā)板，傳感器是從馬家買(mǎi)的現(xiàn)成模塊，采用飛線的形式固定在開(kāi)發(fā)板上（后期會(huì)重新設(shè)計(jì)一款小的適合飛控的板子）

主控+傳感器

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

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

勉強(qiáng)能扣好

【硬件設(shè)計(jì)】

系統(tǒng)框圖

硬件設(shè)計(jì)解析

A、傳感器部分：

各個(gè)傳感器接到IMX-RT1052的SPI3上，進(jìn)行分時(shí)操作。

B、SBUS輸入

由于SBUS信號(hào)是一個(gè)反向電平的串口信號(hào)，這里使用一個(gè)三級(jí)管搭了一個(gè)簡(jiǎn)單的反向電路，從而將信號(hào)正確到接入到處理器的串口輸入端。

C、PWM 信號(hào)輸出

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

D、GPS

GPS模塊是一個(gè)獨(dú)立的單元，通過(guò)串口接到主控的串口5上，上圖中的原理圖是我早期時(shí)候設(shè)計(jì)的基于Ublox M8N的GPS模塊，這里剛好用到。

【軟件設(shè)計(jì)】

軟件設(shè)計(jì)流程

整個(gè)AutoQuad源碼是一個(gè)較大的系統(tǒng)，我這里抽絲剝繭，將其中的主脈絡(luò)流程貼出來(lái)：

關(guān)鍵代碼解析

此版和之前在stm32f405上的版本最大的區(qū)別在于加速度+陀螺儀傳感器、磁力計(jì)、高度計(jì)的數(shù)據(jù)讀取上。原版直接使用spi進(jìn)行驅(qū)動(dòng)，這個(gè)版本我使用了RT-Thread的SPI設(shè)備驅(qū)動(dòng)框架來(lái)進(jìn)行數(shù)據(jù)讀取。

這里將加速度傳感器&陀螺儀驅(qū)動(dòng)源碼列出來(lái)，進(jìn)行一個(gè)簡(jiǎn)單解析：

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

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

此段代碼表示將icm20602作為spi3上的第三個(gè)設(shè)備和spi總線進(jìn)行關(guān)聯(lián)，并使用 GPIO_AD_B1_05 作為其cs引腳（其中64代表 GPIO_AD_B1_05 ，即icm20602的cs引腳是 GPIO_AD_B1_05 ）。

2、配置SPI總線相關(guān)參數(shù)

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、對(duì)總線的互斥操作

為保證多個(gè)設(shè)備對(duì)spi3的互斥使用，需要對(duì)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、寄存器的讀寫(xiě)操作

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、讀取加速度和陀螺儀數(shù)據(jù)

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; }

因?yàn)?icm20602_read_buffer 函數(shù)內(nèi)部已經(jīng)加入了互斥，所以讀取時(shí)不再需要互斥操作。

RT-Thread 使用情況

1、動(dòng)態(tài)內(nèi)存管理；

2、GPIO設(shè)備驅(qū)動(dòng)架構(gòu)；

3、RTC設(shè)備驅(qū)動(dòng)架構(gòu)；

4、SPI設(shè)備驅(qū)動(dòng)架構(gòu)；

5、SDIO設(shè)備驅(qū)動(dòng)架構(gòu)；

6、串口設(shè)備驅(qū)動(dòng)架構(gòu)；

7、MSH命令行；

8、FatFs文件系統(tǒng)。