最近在学习autoquad源码,发现网络上关于这方面的知识不多。大多是笼统的简介并没有对源码的详细分析。所以一切都要从头看起。相比于apm和px4这些飞控代码autoquad要精简的多。因为它不需要兼容多个硬件所以就少了很多的中间层次结构。这也给分析代码带来了很大的便捷。并且其底层使用ST标准外设库,也是国内大多数搞STM32的工程师所熟悉的。下表列举一些autoquad的一些软硬件参数表。
硬件资源 | 详细 |
---|---|
主控 | STM32F405RGT6 |
IMU | MPU6000+HMC5983+MS5611(均为SPI接口) |
存储设备 | SDcard+EEPROM |
通讯接口 | 多路USART、CAN总线、USB接口 |
以下是软件资源
软件资源 | 详细 |
---|---|
操作系统 | COOCOX |
文件系统 | FATFS |
状态估计算法 | SRCDKF |
控制算法 | 传统PID +L1自适应(闭源) |
调用库 | ST标准外设库、USB驱动、FATFS文件系统 |
总体来说autoquad的代码结构还是比较清晰的。包含各个stm32的外设驱动及各个模块的驱动再结合算法部分便形成了完整的工程。由于其编译环境不是keil所以网上也有把该工程移植到keil下的代码。但好多真的是下载后无法运行的。因为好多参数都需要改。不修改的话程序会卡死。
autoquad对应的飞控有模拟传感器和数字传感器。不过研究比较多的是基于其数字传感器的飞控-autoquad-M4。传感器接口都是SPI通信,保证了传感器传输速度的高速型,其完善的SPI驱动结构设计加上DMA传输使得传感器数据采集完全在后台采集,占用极少的系统资源,同时又保证了数据的实时性。
接下来就直接从main函数走起
int main ( void )
{
fpuInit(); // 设置FPU
rccConfiguration();//时钟使能及板载LED初始化
NVIC_PriorityGroupConfig ( NVIC_PriorityGroup_2 ); //中断优先级分组2
CoInitOS();//OS初始化并且创建CoIdleTask
//开辟堆空间以用来初始化,该段空间使用CCM内存
aqInitStack = aqStackInit ( AQINIT_STACK_SIZE, "INIT" );
//创建初始化任务
CoCreateTask ( aqInit, ( void * ) , AQINIT_PRIORITY, &aqInitStack[AQINIT_STACK_SIZE - ], AQINIT_STACK_SIZE );
//开启任务调度
CoStartOS();
return ;
}
这里主要完成几件事情。首先配置了FPU,然后进行RCC配置。这里RCC配置直接使能了所有要用到的外设时钟,这样就不用为以后忘记打开时钟而烦恼了,(反正我总是配置完外设忘记打开时钟)然后设置中断优先级分组为2,调用COOCOX默认初始化函数CoInitOS,初始化并且创建CoIdleTask。即coocox的空闲任务。最后开辟内存并且创建初始化任务aqInit,该任务包含了autoquad几乎所有的初始化代码,以后会慢慢分析。值得一提的是autoquad内存开辟的是CCM内存,可能很多人不太了解这块内存,如果仔细看stm32的手册你会发现对于F4芯片其内部是有一块64K的ram空间的,这块内存称之为CCM内存。其中IRAM1大小为0x20000即128K大小,可以看到其起始地址为0x20000000,IAM2即为ccm内存。其起始地址位0x10000000,大小为0x10000,即64K。关于如何使用CCM内存可以参考如下地址:CCM使用
下面主要进入初始化任务,即aqInit函数。下面是该函数具体内容
void aqInit(void *pdata)
{
#ifdef EEPROM_CS_PORT
// TODO: clean this up
digitalPin *eepromCS = digitalInit(EEPROM_CS_PORT, EEPROM_CS_PIN, );
#endif
#ifdef DAC_TP_PORT
tp = digitalInit(DAC_TP_PORT, DAC_TP_PIN, );
#endif
rtcInit(); // 先初始化RTC再初始化timer,因为RTC校准需要用到定时器
timerInit(); // 初始化US级定时器以用来计时
//sdioLowLevelInit(); //底层SDIO初始化
// filerInit(); //初始化文件系统 创建文件管理任务
supervisorInit(); //信号管理 创建信号管理任务
configInit(); //加载或者存储参数到flash
commInit(); //串口及USB接口初始化
consoleInit(); //控制台初始化
AQ_CONSOLE("console init over\r\n");
#ifdef USE_MAVLINK
mavlinkInit(); //初始化mavlink
#endif
telemetryInit(); //注册数传对应的函数
imuInit(); //IMU模块初始化 创建IMU任务
analogInit(); //电压采集初始化
navUkfInit(); //导航无迹卡尔曼初始化
altUkfInit(); //高度无迹卡尔曼初始化
radioInit(); //接收机初始化 创建接收机解析任务
gpsInit(); //GPS初始化 创建GPS任务解析GPS数据
navInit(); //初始化导航数据
commandInit(); //初始化命令解析,即注册对应的回调函数
#ifdef USE_SIGNALING
signalingInit(); //初始化信号灯
#endif
loggerInit(); //日志初始化
#ifdef CAN_CALIB
canCalibInit(); //CAN 校准初始化
#else
// canInit(); //CAN 初始化
#endif
motorsInit(); //电机初始化
controlInit(); //控制初始化 创建控制任务
// gimbalInit(); //万向节初始化
runInit(); //初始化RUN任务 创建RUN任务
info(); //打印一些飞控信息如mavlink ID、内存使用情况
supervisorInitComplete();//飞控状态初始化
// allow tasks to startup
yield(); //挂起任务等待启动
AQ_NOTICE("Initialization complete, READY.\n");//打印初始化完毕信息
AQ_CONSOLE("Initialization complete, READY.\r\n");
// 启动完成,降低该任务的优先级
if (commData.commTask)
CoSetPriority(commData.commTask, COMM_PRIORITY);
// 使能数传
telemetryEnable();
// 重置空闲任务周期,初始化一个比较大的数,来统计小的周期
minCycles = ;
CoExitTask();//删除该任务
}
这里大部分是初始化任务,包括底层外设的初始化也包含系统任务的创建及初始化。这里主要概括其主要任务的创建。
filerInit()
文件系统任务:主要管理文件的读写等操作
commInit()
通讯任务:主要负责飞控和外界通讯。包含通讯接口有串口、USB、CAN通信
imuInit()
IMU任务:主要负责IMU的初始化及数据采集,根据用户配置不同的IMU
radioInit()
接收机任务:主要处理接收机数据,根据配置接收不同类型的接收机信号
gpsInit()
GPS任务:主要负责处理接收GPS的数据
controlInit()
控制任务:使用控制算法进行飞行器控制,可以配置为PID控制或者L1自适应,L1不开源
runInit()
主要运行任务:运行SRCDKF算法估计当前飞行器状态
这便是autoquad所有的运行任务了,最后初始化完成后便删除了该初始化任务还有一些其他的初始化函数,如定时器初始化、RTC实时时钟初始化、AD采样等属于单片机底层外设初始化,也是在aqInit 初始化函数里完成。
以上只是对autoquad任务有个初始认识。而真正的精妙在于其内部的任务调度、内存管理、数据结构设计、模式设计等。当然还有最重要的状态估计算法及控制算法。这是关于autoquad的首次分析尝试。以后可能会具体到一些零碎的细节。如SPI驱动的设计、IMU的读取及滤波及通讯模式的设计。这些问题是我现在正在分析的。