ANO匿名飞控STM32代码解读(一)任务调度——Ano_Scheduler.c

我所学习的代码是匿名飞控使用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]

发布了2 篇原创文章 · 获赞 0 · 访问量 16

猜你喜欢

转载自blog.csdn.net/jiaruochun/article/details/104260728