我所学习的代码是匿名飞控使用STM32芯片ANO_PioneerPro-20190825的版本。
匿名飞控的整体代码是跑裸机的,任务调度是用STM32F4芯片中的系统时钟计时,做了一个任务调度系统,举个例子,大概就是有四个任务,每个任务分担不同,分别有姿态检测、电机控制等任务。任务一每1ms运行一次、任务二10ms运行一次、任务三1000ms运行一次。在while死循环中不停执行这些任务并且判断这些任务的执行时间间隔,如果达到最小间隔,则再次执行。
我们来看代码,首先这部分代码的c语言基础知识一定要掌握,对于初学者如果你的c语言基础不好,一定去了解了基本语法再来读代码,否则只能靠猜。语法分别涉及到数组、指针、结构体、结构体数组等,基本语法、stm32库函数、关键字等等更不用说了,并且代码中使用了大量的标志位。匿名的代码还是比较简洁易懂的(除了变量命名,有些真是看不懂啊,alt和att后来才懂)。
我把我添加的注释写到了下方的代码中
int main(void)
{
flag.start_ok = All_Init(); //进行所有设备的初始化,并将初始化结果保存
Scheduler_Setup(); //调度器初始化,系统为裸奔,这里人工做了一个时分调度器
while(1)
{
Scheduler_Run(); //运行任务调度器,所有系统功能,除了中断服务函数,都在任务调度器内完成
}
}
All_Init()函数顾名思义就是进行了时钟、外设等的初始化。
再来看Scheduler_Setup(),
认识这个函数前需要先看头文件中的一个结构体
typedef struct
{
void(*task_func)(void); //函数指针
uint16_t rate_hz;
uint16_t interval_ticks;
uint32_t last_run;
}sched_task_t;
这个结构体就是用来储存每个任务的任务函数、执行频率、间隔时间、最后一次运行时刻的。
那么由这个结构体又定义了一个结构体数组
static sched_task_t sched_tasks[] =
{
{Loop_1000Hz,1000, 0, 0},
{Loop_500Hz , 500, 0, 0},
{Loop_200Hz , 200, 0, 0},
{Loop_100Hz , 100, 0, 0},
{Loop_50Hz , 50, 0, 0},
{Loop_20Hz , 20, 0, 0},
{Loop_2Hz , 2, 0, 0},
};
用作者的话说就是//系统任务配置,创建不同执行频率的“线程”
。
接下来我们看到一段代码
//根据数组长度,判断线程数量
#define TASK_NUM (sizeof(sched_tasks)/sizeof(sched_task_t))
这个主要是sizeof
的用法,不太了解可以参见 sizeof()用法总结
这个TASK_SUM
的值就是本次循环有几个任务又要开始执行了。
接下来的代码用TICK_PER_SECOND
除了一下每个任务的频率,算出来周期,如果周期小于1ms就令其等于1ms。具体用来干什么我们应该在后面会看到。
void Scheduler_Setup(void)
{
uint8_t index = 0;
//初始化任务表
for(index=0;index < TASK_NUM;index++)
{
//计算每个任务的延时周期数
sched_tasks[index].interval_ticks = TICK_PER_SECOND/sched_tasks[index].rate_hz;
//最短周期为1,也就是1ms
if(sched_tasks[index].interval_ticks < 1)
{
sched_tasks[index].interval_ticks = 1;
}
}
}
最后一个函数我想程序作者已经说得很清楚了。
void Scheduler_Setup(void)
{
uint8_t index = 0;
//初始化任务表
for(index=0;index < TASK_NUM;index++)
{
//计算每个任务的延时周期数
sched_tasks[index].interval_ticks = TICK_PER_SECOND/sched_tasks[index].rate_hz;
//最短周期为1,也就是1ms
if(sched_tasks[index].interval_ticks < 1)
{
sched_tasks[index].interval_ticks = 1;
}
}
}
剩余部分就是封装好的任务函数了。
static void Loop_1000Hz(void) //1ms执行一次
{
test_dT_1000hz[0] = test_dT_1000hz[1];
test_rT[3] = test_dT_1000hz[1] = GetSysTime_us ();
test_dT_1000hz[2] = (u32)(test_dT_1000hz[1] - test_dT_1000hz[0]) ;
//////////////////////////////////////////////////////////////////////
/*传感器数据读取*/
Fc_Sensor_Get();
/*惯性传感器数据准备*/
Sensor_Data_Prepare(1);
/*姿态解算更新*/
IMU_Update_Task(1);
/*获取WC_Z加速度*/
WCZ_Acc_Get_Task();
WCXY_Acc_Get_Task();
/*飞行状态任务*/
Flight_State_Task(1,CH_N);
/*开关状态任务*/
Swtich_State_Task(1);
/*光流融合数据准备任务*/
ANO_OF_Data_Prepare_Task(0.001f);
/*数传数据交换*/
ANO_DT_Data_Exchange();
//////////////////////////////////////////////////////////////////////
test_rT[4]= GetSysTime_us ();
test_rT[5] = (u32)(test_rT[4] - test_rT[3]) ;
}
static void Loop_500Hz(void) //2ms执行一次
{
/*姿态角速度环控制*/
Att_1level_Ctrl(2*1e-3f);
/*电机输出控制*/
Motor_Ctrl_Task(2);
/*UWB数据获取*/
Ano_UWB_Get_Data_Task(2);
}
static void Loop_200Hz(void) //5ms执行一次
{
/*获取姿态角(ROLL PITCH YAW)*/
calculate_RPY();
/*姿态角度环控制*/
Att_2level_Ctrl(5e-3f,CH_N);
}
static void Loop_100Hz(void) //10ms执行一次
{
test_rT[0]= GetSysTime_us ();
//////////////////////////////////////////////////////////////////////
/*遥控器数据处理*/
RC_duty_task(10);
/*飞行模式设置任务*/
Flight_Mode_Set(10);
/*高度数据融合任务*/
WCZ_Fus_Task(10);
GPS_Data_Processing_Task(10);
/*高度速度环控制*/
Alt_1level_Ctrl(10e-3f);
/*高度环控制*/
Alt_2level_Ctrl(10e-3f);
/*--*/
AnoOF_DataAnl_Task(10);
/*灯光控制*/
LED_Task2(10);
//////////////////////////////////////////////////////////////////////
test_rT[1]= GetSysTime_us ();
test_rT[2] = (u32)(test_rT[1] - test_rT[0]) ;
}
static void Loop_50Hz(void) //20ms执行一次
{
/*罗盘数据处理任务*/
Mag_Update_Task(20);
/*程序指令控制*/
FlyCtrl_Task(20);
//
ANO_OFDF_Task(20);
// /*UWB数据计算*/
// Ano_UWB_Data_Calcu_Task(20);
/*位置速度环控制*/
Loc_1level_Ctrl(20,CH_N);
/*OPMV检测是否掉线*/
OpenMV_Offline_Check(20);
/*OPMV色块追踪数据处理任务*/
ANO_CBTracking_Task(20);
/*OPMV寻线数据处理任务*/
ANO_LTracking_Task(20);
/*OPMV控制任务*/
ANO_OPMV_Ctrl_Task(20);
}
static void Loop_20Hz(void) //50ms执行一次
{
/*电压相关任务*/
Power_UpdateTask(50);
//恒温控制
Thermostatic_Ctrl_Task(50);
}
static void Loop_2Hz(void) //500ms执行一次
{
/*延时存储任务*/
Ano_Parame_Write_task(500);
}
如有错误还请不吝指正,欢迎交流,我的邮箱是[email protected]。