教你i.MX RT1052的开源AutoQuad飞控

发布时间:2024-06-25  

【背景描述】

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文件系统。


文章来源于:电子工程世界    原文链接
本站所有转载文章系出于传递更多信息之目的,且明确注明来源,不希望被转载的媒体或个人可与我们联系,我们将立即进行删除处理。

我们与500+贴片厂合作,完美满足客户的定制需求。为品牌提供定制化的推广方案、专属产品特色页,多渠道推广,SEM/SEO精准营销以及与公众号的联合推广...详细>>

利用葫芦芯平台的卓越技术服务和新产品推广能力,原厂代理能轻松打入消费物联网(IOT)、信息与通信(ICT)、汽车及新能源汽车、工业自动化及工业物联网、装备及功率电子...详细>>

充分利用其强大的电子元器件采购流量,创新性地为这些物料提供了一个全新的窗口。我们的高效数字营销技术,不仅可以助你轻松识别与连接到需求方,更能够极大地提高“闲置物料”的处理能力,通过葫芦芯平台...详细>>

我们的目标很明确:构建一个全方位的半导体产业生态系统。成为一家全球领先的半导体互联网生态公司。目前,我们已成功打造了智能汽车、智能家居、大健康医疗、机器人和材料等五大生态领域。更为重要的是...详细>>

我们深知加工与定制类服务商的价值和重要性,因此,我们倾力为您提供最顶尖的营销资源。在我们的平台上,您可以直接接触到100万的研发工程师和采购工程师,以及10万的活跃客户群体...详细>>

凭借我们强大的专业流量和尖端的互联网数字营销技术,我们承诺为原厂提供免费的产品资料推广服务。无论是最新的资讯、技术动态还是创新产品,都可以通过我们的平台迅速传达给目标客户...详细>>

我们不止于将线索转化为潜在客户。葫芦芯平台致力于形成业务闭环,从引流、宣传到最终销售,全程跟进,确保每一个potential lead都得到妥善处理,从而大幅提高转化率。不仅如此...详细>>