【持续更新中...】2021年全国大学生电子设计大赛 （三）匿名四轴拓空者飞控系统设计解读

模块装备图：

总览：

接口功能图（一）

模块功能图（二）正面

模块功能图（三）背面

芯片原理图:

总体分析与解析

以一键起飞加定高为例：

头文件目录

#include "Ano_Scheduler.h"
#include "Drv_Bsp.h"
#include "Drv_icm20602.h"
#include "Ano_LED.h"
#include "Ano_FlightDataCal.h"
#include "Ano_Sensor_Basic.h"

#include "Drv_gps.h"
#include "Ano_DT.h"
#include "Ano_RC.h"
#include "Ano_Parameter.h"
#include "Drv_led.h"
#include "Drv_ak8975.h"
#include "Drv_spl06.h"
#include "Ano_FlightCtrl.h"
#include "Ano_AttCtrl.h"
#include "Ano_LocCtrl.h"
#include "Ano_AltCtrl.h"
#include "Ano_MotorCtrl.h"
#include "Ano_Parameter.h"
#include "Ano_MagProcess.h"
#include "Ano_Power.h"
#include "Ano_OF.h"
#include "Drv_heating.h"
#include "Ano_FlyCtrl.h"
#include "Ano_UWB.h"
#include "Drv_OpenMV.h"
#include "Ano_OPMV_CBTracking_Ctrl.h"
#include "Ano_OPMV_LineTracking_Ctrl.h"
#include "Ano_OPMV_Ctrl.h"
#include "Ano_OF_DecoFusion.h"
#include "Drv_mv.h"

灯光闪烁函数： INT_1ms_Task()

void INT_1ms_Task()
{
    	
// if(fc_sta.start_ok == 0) return;
	
	//标记1ms执行
	lt0_run_flag ++;
	//灯光驱动
	LED_1ms_DRV();

	//循环计数
	circle_cnt[0] ++;
	//20次循环
	circle_cnt[0] %= CIRCLE_NUM;
	//
	if(!circle_cnt[0])
	{
    
	}
}

所有传感器读取函数： Loop_Task_0()

static void Loop_Task_0()//1ms执行一次
{
    
	// 
	/*传感器数据读取*/
	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();	
}

姿态环以及电机初始化：Loop_Task_1

static void Loop_Task_1(u32 dT_us)	//2ms执行一次
{
    
// 
	float t1_dT_s;
	t1_dT_s = (float)dT_us *1e-6f;
	//========================
	/*姿态角速度环控制*/
	Att_1level_Ctrl(2*1e-3f);
	
	/*电机输出控制*/
	Motor_Ctrl_Task(2);	

// 
}

姿态环角控制

static void Loop_Task_2(u32 dT_us)	//6ms执行一次
{
    
// 
	float t2_dT_s;
	t2_dT_s = (float)dT_us *1e-6f;
	//========================
	/*获取姿态角（ROLL PITCH YAW）*/
	calculate_RPY();
	
   User_my_yaw_2level(6,line);	//寻线YAW修正
	/*姿态角度环控制*/
	Att_2level_Ctrl(6e-3f,CH_N);

	//
	
// 
}

飞行任务设置：void Loop_Task_5

static void Loop_Task_5(u32 dT_us)	//11ms执行一次
{
    	
// 
	float t2_dT_s = (float)dT_us *1e-6f;//0.008f;//
	//========================
	/*遥控器数据处理*/
	RC_duty_task(11);
	
	/*飞行模式设置任务*/
	Flight_Mode_Set(11);
	

	
	/*高度数据融合任务*/
	WCZ_Fus_Task(11);
	GPS_Data_Processing_Task(11);
	
	/*高度速度环控制*/
	Alt_1level_Ctrl(11e-3f);
	
	/*高度环控制*/
	Alt_2level_Ctrl(11e-3f);
	
	/*--*/	
	AnoOF_DataAnl_Task(11);

	/*灯光控制*/	
	LED_Task2(11);


// 
}

特定飞行任务函数：Loop_Task_8

extern struct _MV_ 		MV;

static void Loop_Task_8(u32 dT_us)	//20ms执行一次
{
    
	u8 dT_ms = 20;//(u8)(dT_us *1e-3f);
	//==========================
	//
	/*罗盘数据处理任务*/
	Mag_Update_Task(20);
	/*程序指令控制*/
	FlyCtrl_Task(20);
	//
	ANO_OFDF_Task(20);
	/*--*/
	Ano_UWB_Data_Calcu_Task(20);
	/*位置速度环控制*/
	Loc_1level_Ctrl(20,CH_N);
	
	
	/*用户*/
	MV_Decoupling(20);  //对数据处理传到飞机 解旋转
	Loc_2level_Ctrl(20,&MV);
	Tracking_Ctrlw(0.02f);      //小飞机改的程序
// Tracking_Ctrl(0.02f); //绕框
	
// Proce0_Ctrl(0.02f); //定点

     匿名程序 
// /*OPMV检测是否掉线*/
// OpenMV_Offline_Check(20);
// /*OPMV色块追踪数据处理任务*/
// ANO_CBTracking_Task(20);
// /*OPMV寻线数据处理任务*/
// ANO_LTracking_Task(20);
// /*OPMV控制任务*/
// ANO_OPMV_Ctrl_Task(20);

}

电压以及温度控制函：Loop_Task_9

static void Loop_Task_9(u32 dT_us)	//50ms执行一次
{
    
	//
	/*电压相关任务*/
	Power_UpdateTask(50);
	//恒温控制（不能直接注释掉，否则开机过不了校准）
	Thermostatic_Ctrl_Task(50);
	// /*延时存储任务*/
	Ano_Parame_Write_task(50);
}

系统任务多线程配置函数：

static sched_task_t sched_tasks[] = 
{
    
	//任务n, 周期us, 上次时间us
	{
    Loop_Task_1 ,  2000,  0 },
	{
    Loop_Task_2 ,  6000,  0 },
// {Loop_Task_2 , 2500, 0 },
// {Loop_Task_3 , 2500, 0 },
// {Loop_Task_4 , 2500, 0 },
	{
    Loop_Task_5 ,  11000,  0 },
// {Loop_Task_6 , 9090, 0 },
// {Loop_Task_7 , 9090, 0 },
	{
    Loop_Task_8 , 20000,  0 },
	{
    Loop_Task_9 , 50000,  0 },
// {Loop_Task_10,100000, 0 },
};

线程执行函数

u8 Main_Task(void)
{
    
	uint8_t index = 0;
	
	//查询1ms任务是否需要执行
	if(lt0_run_flag!=0)
	{
    
		//
		lt0_run_flag--;
		Loop_Task_0();
	}
	//循环判断其他所有线程任务，是否应该执行
	uint32_t time_now,delta_time_us;
	for(index=0;index < TASK_NUM;index++)
	{
    
		//获取系统当前时间，单位US
		 time_now = GetSysRunTimeUs();//SysTick_GetTick();
		//进行判断，如果当前时间减去上一次执行的时间，大于等于该线程的执行周期，则执行线程
		if(time_now - sched_tasks[index].last_run >= sched_tasks[index].interval_ticks)
		{
    
			delta_time_us = (u32)(time_now - sched_tasks[index].last_run);

			//更新线程的执行时间，用于下一次判断
			sched_tasks[index].last_run = time_now;
			//执行线程函数，使用的是函数指针
			sched_tasks[index].task_func(delta_time_us);

		}	 
	}
	
	return 0;
}

初始化函数结构分析：

LED初始化： Dvr_LedInit(void)

分析：可以看到初始化函数由两个部分构成
- 第一部分：ROM_SysCtlPeripheralEnable （GPIO口 ）
- 第二部分：ROM_GPIOPinTypeGPIOOutput（GPIO口 ，GPIO引脚 ）

#define LED1_SYSCTL SYSCTL_PERIPH_GPIOD
#define LED2_SYSCTL SYSCTL_PERIPH_GPIOD
#define LED3_SYSCTL SYSCTL_PERIPH_GPIOA
#define LEDS_SYSCTL SYSCTL_PERIPH_GPIOF
#define LED1_PORT GPIOD_BASE
#define LED2_PORT GPIOD_BASE
#define LED3_PORT GPIOA_BASE
#define LEDS_PORT GPIOF_BASE
#define LED1_PIN GPIO_PIN_0
#define LED2_PIN GPIO_PIN_1
#define LED3_PIN GPIO_PIN_6
#define LEDS_PIN GPIO_PIN_4
void Dvr_LedInit(void)
{
    
		ROM_SysCtlPeripheralEnable(LED1_SYSCTL);
		ROM_SysCtlPeripheralEnable(LED2_SYSCTL);
		ROM_SysCtlPeripheralEnable(LED3_SYSCTL);
		ROM_SysCtlPeripheralEnable(LEDS_SYSCTL);
		ROM_GPIOPinTypeGPIOOutput(LED1_PORT, LED1_PIN);
		ROM_GPIOPinTypeGPIOOutput(LED2_PORT, LED2_PIN);
		ROM_GPIOPinTypeGPIOOutput(LED3_PORT, LED3_PIN);
		ROM_GPIOPinTypeGPIOOutput(LEDS_PORT, LEDS_PIN);
	Drv_LedOnOff(LED_B, 1);
}

传感器数据读取函数

解析 ：飞控三大传感器数据

• 陀螺仪 加速度
• 电子罗盘
• 气压计
• 这些函数以及封装好了 为的是方便我们解析出这个程序

void Fc_Sensor_Get()//1ms
{
    
	static u8 cnt;
	if(flag.start_ok)
	{
    
		Drv_Icm20602_Read(); //陀螺仪 加速度计
		
		cnt ++;
		cnt %= 20;
		if(cnt==0)
		{
    
			Drv_AK8975_Read(); // 电子罗盘磁力计数据
			baro_height = (s32)Drv_Spl0601_Read(); //读取气压计的数据
		}
	}	
	test_time_cnt++;

}

陀螺仪加速度提取：Drv_Icm20602_Read

void Drv_Icm20602_Read( void )
{
    
	//读取传感器寄存器，连续读14个字节
	icm20602_readbuf(MPUREG_ACCEL_XOUT_H,14,mpu_buffer);
	//数据赋值
	ICM_Get_Data();
}

电子罗盘磁力计数据：Drv_AK8975_Read();

void Drv_AK8975_Read(void)
{
    	
	
	ak8975_enable(1);
	Drv_Spi0SingleWirteAndRead(AK8975_HXL_REG|0x80);
	for(u8 i=0; i<6; i++)
		ak8975_buf[i] = Drv_Spi0SingleWirteAndRead(0xff);
	ak8975_enable(0);
	
	ak8975_Trig();
	
}

读取气压计的数据：(s32)Drv_Spl0601_Read()

float Drv_Spl0601_Read ( void )
{
    


    spl0601_get_raw_temp();
    temperature = spl0601_get_temperature();
    spl0601_get_raw_pressure();
    baro_pressure = spl0601_get_pressure();
    alt_3 = ( 101400 - baro_pressure ) / 1000.0f;
    height = 0.82f * alt_3 * alt_3 * alt_3 + 0.09f * ( 101400 - baro_pressure ) * 100.0f 
    alt_high = ( height - baro_Offset ) ; //cm +
    return alt_high;
}

惯性传感器检测函数

静止检测函数：motionless_check(dT_ms);

解析: 通过判断T来决定是否是静止状态
而T的判断标准是 原数据减去旧的角度数据

void Sensor_Basic_Init()
{
    
  # 重新相对传感器的偏移量
	Center_Pos_Set();
	
	sensor.acc_z_auto_CALIBRATE = 1; # 开机自动校准对准Z轴
	sensor.gyr_CALIBRATE = 2;  # 开机自动校准陀螺仪
}
void motionless_check(u8 dT_ms)
{
    
	u8 t = 0;
	for(u8 i = 0;i<3;i++)
	{
    
		g_d_sum[i] += 3*ABS(sensor.Gyro_Original[i] - g_old[i]) ;
		g_d_sum[i] -= dT_ms ;	
		g_d_sum[i] = LIMIT(g_d_sum[i],0,200);
		if( g_d_sum[i] > 10)
		{
    
			t++;
		}
		g_old[i] = sensor.Gyro_Original[i];
	}
	if(t>=2)
	{
    
		flag.motionless = 0;	
	}
	else
	{
    
		flag.motionless = 1;
	}

}

陀螺仪MPU6050 函数 MPU6050_Data_Offset();

static u8 off_cnt;
		if(sensor.gyr_CALIBRATE || sensor.acc_CALIBRATE || sensor.acc_z_auto_CALIBRATE)
	{
    		
		if(flag.motionless == 0 || sensor_val[A_Z]<(GRAVITY_ACC_PN16G/2) || (flag.mems_temperature_ok == 0))
		{
    
				gyro_sum_cnt = 0;
				acc_sum_cnt=0;
				acc_z_auto_cnt = 0;
				
				for(u8 j=0;j<3;j++)
				{
    
					acc_auto_sum_temp[j] = sum_temp[G_X+j] = sum_temp[A_X+j] = 0;
				}
				sum_temp[TEM] = 0;
		}
		off_cnt++;			
		if(off_cnt>=10)
		{
    	
			off_cnt=0;			
			if(sensor.gyr_CALIBRATE)
			{
    
				LED_STA_CALI_GYR = 1;
				gyro_sum_cnt++;
				for(u8 i = 0;i<3;i++)
				{
    
					sum_temp[G_X+i] += sensor.Gyro_Original[i];
				}
				if( gyro_sum_cnt >= OFFSET_AV_NUM )
				{
    
					LED_STA_CALI_GYR = 0;
					for(u8 i = 0;i<3;i++)
					{
    
						save.gyro_offset[i] = (float)sum_temp[G_X+i]/OFFSET_AV_NUM;
						sum_temp[G_X + i] = 0;
					}
					gyro_sum_cnt =0;
					if(sensor.gyr_CALIBRATE == 1)
					{
    
						if(sensor.acc_CALIBRATE == 0)
						{
    
							data_save();
						}
					}
					sensor.gyr_CALIBRATE = 0;
// ANO_DT_SendString("GYR init OK!");

				}
			}
			
			if(sensor.acc_CALIBRATE == 1)
			{
    
				LED_STA_CALI_ACC = 1;
				acc_sum_cnt++;
				sum_temp[A_X] += sensor_val_rot[A_X];
				sum_temp[A_Y] += sensor_val_rot[A_Y];
				sum_temp[A_Z] += sensor_val_rot[A_Z] - GRAVITY_ACC_PN16G;// - 65535/16; // +-8G
				sum_temp[TEM] += sensor.Tempreature;

				if( acc_sum_cnt >= OFFSET_AV_NUM )
				{
    
					LED_STA_CALI_ACC = 0;
					for(u8 i=0 ;i<3;i++)
					{
    
						save.acc_offset[i] = sum_temp[A_X+i]/OFFSET_AV_NUM;
						sum_temp[A_X + i] = 0;
					}
					acc_sum_cnt =0;
					sensor.acc_CALIBRATE = 0;
// ANO_DT_SendString("ACC init OK!");
					data_save();
				}	
			}
		}
	}
}

惯性传感器转换单位 函数段

	for(u8 i =0 ;i<3;i++)
		{
    
		 # 陀螺仪转换到度每秒 量程+-2000度
			sensor.Gyro_deg[i] = sensor.Gyro[i] *0.061036f ;
		# 陀螺仪转换到弧度每秒 量程+-2000度
			sensor.Gyro_rad[i] = sensor.Gyro_deg[i] *0.01745f;//sensor.Gyro[i] 
		# 加速度计转换成厘米 每平方秒 量程 +- 8G
			sensor.Acc_cmss[i] = (sensor.Acc[i] *RANGE_PN16G_TO_CMSS );// /65535 * 16*981; +-8G 
		}

姿态解算更新函数 IMU_Update_Task(1);

作用： 重力传感器 以及 磁力计 进行姿态计算
注释如下： 如果准备飞行 复位重力标记和磁力计复位标记
校准陀螺仪 不保存
自动复位
已经置位复位标记
设置重力互补融合修正Kp系数
设置重力互补融合修正ki系数
设置罗盘互补融合修正ki系数
磁力计修正使能
姿态计算 更新 融合

void IMU_Update_Task(u8 dT_ms)
{
    
// 如果准备飞行 复位重力标记和磁力计复位标记 
   			if(flag.unlock_sta )
   			{
    
   				imu_state.G_reset = imu_state.M_reset = 0;
   				reset_imu_f = 0;
   			}
   			else 
   			{
    
   				if(flag.motionless == 0)
   				{
    
// imu_state.G_reset = 1;//自动复位
   					//sensor.gyr_CALIBRATE = 2;
   				}	
   				
   				if(reset_imu_f==0 )//&& flag.motionless == 1)
   				{
    
   					imu_state.G_reset = 1;//自动复位
   					sensor.gyr_CALIBRATE = 2;//校准陀螺仪 不保存
   					reset_imu_f = 1;     //已经置位复位标记
   				}
   			}				
   			if(0) 
   			{
    
   				imu_state.gkp = 0.0f;
   				imu_state.gki = 0.0f;
   			}
   			else
   			{
    
   				if(0)
   				{
    
   					imu_state.gkp = 0.2f;
   				}
   				else
   				{
    
   					//设置重力互补融合修正Kp系数
   					imu_state.gkp = 0.2f;//0.4f;
   				}
   				// 设置重力互补融合修正ki系数
   				imu_state.gki = 0.01f;
   			// 设置罗盘互补融合修正ki系数
   				imu_state.mkp = 0.1f;
   			}
   			imu_state.M_fix_en = sens_hd_check.mag_ok;		//磁力计修正使能
   			//姿态计算 更新 融合
   			IMU_update(dT_ms *1e-3f, &imu_state,sensor.Gyro_rad, sensor.Acc_cmss, mag.val,&imu_data);//x3_dT_1[2] * 0.000001f
}

获取加速度函数 (一)WCZ_Acc_Get_Task();

获得 Z 轴上的加速度
注释： 获取最小周期

void WCZ_Acc_Get_Task()//最小周期
{
    
	wcz_acc_use += 0.03f *(imu_data.w_acc[Z] - wcz_acc_use);
}

获取加速度函数 (二)WCXY_Acc_Get_Task();

获得 X Y轴上的加速度
注释： 最小周期

void WCXY_Acc_Get_Task(void)//最小周期
{
    
	wcx_acc_use += 0.015f *(imu_data.w_acc[X] - wcx_acc_use);
	wcy_acc_use += 0.015f *(imu_data.w_acc[Y] - wcy_acc_use);
}

任务调度：

飞行状态任务：Flight_State_Task(1,CH_N)

飞行状态任务调度：
注释如下：

• 设置油门摇杆量
• 推油门启动
• 起飞1秒 后 认为已经在飞行
• 设置 上升速度
• 设置 下降速度
• 飞控系统 z速度目标标量综合设定
• 速度设定量 正负 参考ANO 坐标参考方向
• 飞控系统 XY速度 目标综合量设定
• 调用检测着陆的函数
• 倾斜过上大锁

void Flight_State_Task(u8 dT_ms,s16 *CH_N)
{
    
	s16 thr_deadzone;
	static float max_speed_lim,vel_z_tmp[2];
	// 设置油门摇杆量
	thr_deadzone = (flag.wifi_ch_en != 0) ? 0 : 50;
	fs.speed_set_h_norm[Z] = my_deadzone(CH_N[CH_THR],0,thr_deadzone) *0.0023f;
	fs.speed_set_h_norm_lpf[Z] += 0.5f *(fs.speed_set_h_norm[Z] - fs.speed_set_h_norm_lpf[Z]);
	// 推油门启动
	if(flag.unlock_sta)
	{
    	
		if(fs.speed_set_h_norm[Z]>0.01f && flag.motor_preparation == 1) // 0-1
		{
    
			flag.taking_off = 1;
		}	
	}		
	fc_stv.vel_limit_z_p = MAX_Z_SPEED_UP;
	fc_stv.vel_limit_z_n = -MAX_Z_SPEED_DW;	
	if( flag.taking_off )
	{
    
			
		if(flying_cnt<1000)//800ms
		{
    
			flying_cnt += dT_ms;
		}
		else
		{
    
	// 起飞1秒 后 认为已经在飞行
			flag.flying = 1;  
		}
		
		if(fs.speed_set_h_norm[Z]>0)
		{
    
  // 设置 上升速度 
			vel_z_tmp[0] = (fs.speed_set_h_norm_lpf[Z] *MAX_Z_SPEED_UP);
		}
		else
		{
    
 // 设置 下降速度
			vel_z_tmp[0] = (fs.speed_set_h_norm_lpf[Z] *MAX_Z_SPEED_DW);
		}
 // 飞控系统 z速度目标标量综合设定
		vel_z_tmp[1] = vel_z_tmp[0] + program_ctrl.vel_cmps_h[Z] + pc_user.vel_cmps_set_z;
				vel_z_tmp[1] = LIMIT(vel_z_tmp[1],fc_stv.vel_limit_z_n,fc_stv.vel_limit_z_p);
		fs.speed_set_h[Z] += LIMIT((vel_z_tmp[1] - fs.speed_set_h[Z]),-0.8f,0.8f);//
	}
	else
	{
    
		fs.speed_set_h[Z] = 0 ;
	}
	float speed_set_tmp[2];
// 速度设定量 正负 参考ANO 坐标参考方向
	fs.speed_set_h_norm[X] = (my_deadzone(+CH_N[CH_PIT],0,50) *0.0022f);
	fs.speed_set_h_norm[Y] = (my_deadzone(-CH_N[CH_ROL],0,50) *0.0022f);
		
	LPF_1_(3.0f,dT_ms*1e-3f,fs.speed_set_h_norm[X],fs.speed_set_h_norm_lpf[X]);
	LPF_1_(3.0f,dT_ms*1e-3f,fs.speed_set_h_norm[Y],fs.speed_set_h_norm_lpf[Y]);
	
	max_speed_lim = MAX_SPEED;
	
	if(switchs.of_flow_on && !switchs.gps_on )
	{
    
		max_speed_lim = 1.5f *wcz_hei_fus.out;
		max_speed_lim = LIMIT(max_speed_lim,50,150);
	}	
	
	fc_stv.vel_limit_xy = max_speed_lim;
	
	//飞控系统 XY速度 目标综合量设定
	speed_set_tmp[X] = fc_stv.vel_limit_xy *fs.speed_set_h_norm_lpf[X] + program_ctrl.vel_cmps_h[X] + pc_user.vel_cmps_set_h[X];
	speed_set_tmp[Y] = fc_stv.vel_limit_xy *fs.speed_set_h_norm_lpf[Y] + program_ctrl.vel_cmps_h[Y] + pc_user.vel_cmps_set_h[Y];
	
	length_limit(&speed_set_tmp[X],&speed_set_tmp[Y],fc_stv.vel_limit_xy,fs.speed_set_h_cms);

	fs.speed_set_h[X] = fs.speed_set_h_cms[X];
	fs.speed_set_h[Y] = fs.speed_set_h_cms[Y];	
	
// 调用检测着陆的函数
	land_discriminat(dT_ms);
	
// 倾斜过上大锁
	if(rolling_flag.rolling_step == ROLL_END)
	{
    
	if(imu_data.z_vec[Z<0.25f)  //75度
	//倾斜过 上大锁
		{
    
			//
			if(mag.mag_CALIBRATE==0)
			{
    
				imu_state.G_reset = 1;
			}
			flag.unlock_cmd = 0;
		}

	}	
		//
//校准中 ，复位重力方向
	if(sensor.gyr_CALIBRATE != 0 || sensor.acc_CALIBRATE != 0 ||sensor.acc_z_auto_CALIBRATE)
	{
    
		imu_state.G_reset = 1;
	}
// 复位重力方向时，认为传感器失效
	if(imu_state.G_reset == 1)
	{
    
		flag.sensor_imu_ok = 0;
		LED_STA.rst_imu = 1;
		WCZ_Data_Reset(); 
		//复位 高度数据融合
	}
	else if(imu_state.G_reset == 0)
	{
    	
		if(flag.sensor_imu_ok == 0)
		{
    
			flag.sensor_imu_ok = 1;
			LED_STA.rst_imu = 0;
			ANO_DT_SendString("IMU OK!");
		}
	}
	
	/*飞行状态复位*/
	if(flag.unlock_sta == 0)
	{
    
		flag.flying = 0;
		landing_cnt = 0;
		flag.taking_off = 0;
		flying_cnt = 0;	
		flag.rc_loss_back_home = 0;
		//复位融合
		if(flag.taking_off == 0)
		{
    
// wxyz_fusion_reset();
		}
	}
	

}

开关状态任务： Swtich_State_Task(1);

开关状态任务作用：判断各个任务状态是否有效

• 光流模块
• 光流质量 大于 60*
• 光流质量 大于50*
• 或者飞行之前
• 认为光流可用。判定可用延迟时间
• 光流高度 600 cm内有效
• 延时 1.5s 判断光流是否有效
• 判定高度无效
• GPS
• UWB
• OPENMV

void Swtich_State_Task(u8 dT_ms)
{
    
	switchs.baro_on = 1;
	// 光流模块
	if(sens_hd_check.of_ok || sens_hd_check.of_df_ok)// 
	{
    
		if(sens_hd_check.of_ok)
		{
    
			jsdata.of_qua = OF_QUALITY;
			jsdata.of_alt = (u16)OF_ALT;
		}
		else if(sens_hd_check.of_df_ok)
		{
    
			jsdata.of_qua = of_rdf.quality;
			jsdata.of_alt = Laser_height_cm;
		}
		
		//
		if(jsdata.of_qua>50 )
		//|| flag.flying == 0) || flag.flying 光流质量 大于 60* // 光流质量 大于50* / 或者飞行之前 // 认为光流可用。判定可用延迟时间
		{
    
			if(of_quality_delay<500)
			{
    
				of_quality_delay += dT_ms;
			}
			else
			{
    
				of_quality_ok = 1;
			}
		}
		else
		{
    
			of_quality_delay =0;
			of_quality_ok = 0;
		}
		
// 光流高度 600 cm内有效
		if(jsdata.of_alt<600)
		{
    
			//
			of_tof_on_tmp = 1;
			jsdata.valid_of_alt_cm = jsdata.of_alt;
// 延时 1.5s 判断光流是否有效
			if(of_alt_delay<1500)
			{
    
				of_alt_delay += dT_ms;			
			}
			else
			{
    
	// 判定高度有效
				of_alt_ok = 1;
			}
		}
		else
		{
    
			// 
			of_tof_on_tmp = 0;
			// 延时1.5秒判断激光高度是否有效
			if(of_alt_delay>0)
			{
    
				of_alt_delay -= dT_ms;	
			}
			else
			{
    
				//判定高度无效
				of_alt_ok = 0;
			}				
		}
		if(flag.flight_mode == LOC_HOLD)
		{
    		
			if(of_alt_ok && of_quality_ok)
			{
    
				switchs.of_flow_on = 1;
			}
			else
			{
    
				switchs.of_flow_on = 0;
			}

		}
		else
		{
    
			of_tof_on_tmp = 0;
			switchs.of_flow_on = 0;
		}	
		switchs.of_tof_on = of_tof_on_tmp;
	}
	else
	{
    
		switchs.of_flow_on = switchs.of_tof_on = 0;
	}
	//
	if(sens_hd_check.tof_ok)
	{
    
		if(0)//(Laser_height_mm<1900)
		{
    
			switchs.tof_on = 1;
		}
		else
		{
    
			switchs.tof_on = 0;
		}
	}
	else
	{
    
		switchs.tof_on = 0;
	}
	//GPS 
	//UWB
	if(uwb_data.online && flag.flight_mode == LOC_HOLD)
	{
    
		switchs.uwb_on = 1;
	}
	else
	{
    
		switchs.uwb_on = 0;
	}
      //OPMV
	if(opmv.offline==0 && flag.flight_mode == LOC_HOLD)
	{
    
		switchs.opmv_on = 1;
	}
	else
	{
    
		switchs.opmv_on = 0;
	}
	}

光流融合数据转换任务： ANO_OF_Data_Prepare_Task(0.001f);

函数名 ANO_OF_DATA_Check_Task
功能说明 ： 光流准备数据任务
参数 周期时间 （s）
返回值 无

void ANO_OF_Data_Prepare_Task(float dT_s)
{
    
	//
	ANO_OF_Data_Get(&dT_s,OF_DATA_BUF);
 OF_INS_Get(&dT_s,RADPS_X,RADPS_Y,imu_data.w_acc[0],imu_data.w_acc[1]);
}

数传数据交换任务：ANO_DT_Data_Exchange();

• 函数名： ANO_DT_Data_Exchange();
• 作用：数传数据交换任务
• 提示：Data_Exchange函数处理各种数据发送请求，比如想实现每5ms发送一次传感器数据至上位机，即在此函数内实现
• 调用时长：此函数应由用户每1ms调用一次

void ANO_DT_Data_Exchange(void)
{
    
	static u16 cnt = 0;
	static u16 senser_cnt 	= 10;
	static u16 senser2_cnt 	= 50;
	static u16 user_cnt 	= 10;
	static u16 status_cnt 	= 15;
	static u16 rcdata_cnt 	= 20;
	static u16 motopwm_cnt	= 20;
	static u16 power_cnt	= 50;
	static u16 speed_cnt   	= 50;
	static u16 sensorsta_cnt = 500;
	static u16 omv_cnt = 100;
	static u16 location_cnt = 500;
	static u8	flag_send_omv = 0;	
	if((cnt % senser_cnt) == (senser_cnt-1))
		f.send_senser = 1;

	if((cnt % senser2_cnt) == (senser2_cnt-1))
		f.send_senser2 = 1;	

	if((cnt % user_cnt) == (user_cnt-2))
		f.send_user = 1;
	
	if((cnt % status_cnt) == (status_cnt-1))
		f.send_status = 1;	
	
	if((cnt % rcdata_cnt) == (rcdata_cnt-1))
		f.send_rcdata = 1;	
	
	if((cnt % motopwm_cnt) == (motopwm_cnt-2))
		f.send_motopwm = 1;	
	
	if((cnt % power_cnt) == (power_cnt-2))
		f.send_power = 1;		
	
	if((cnt % speed_cnt) == (speed_cnt-3))
		f.send_speed = 1;		
	
	if((cnt % sensorsta_cnt) == (sensorsta_cnt-2))
	{
    
		f.send_sensorsta = 1;		
	}	
	
	if((cnt % omv_cnt) == (omv_cnt-2))
	{
    
		flag_send_omv = 1;		
	}	
	
	if((cnt % location_cnt) == (location_cnt-3))
	{
    
		f.send_location = 1;		
	}
	
	if(++cnt>1000) cnt = 0;
	if(f.send_version)
	{
    
		f.send_version = 0;
		ANO_DT_Send_Version(4,300,100,400,0);
	}
	else if(f.paraToSend < 0xffff)
	{
    
		ANO_DT_SendParame(f.paraToSend);
		f.paraToSend = 0xffff;
	}
	else if(f.send_status)
	{
    
		f.send_status = 0;
		ANO_DT_Send_Status(imu_data.rol,imu_data.pit,imu_data.yaw,wcz_hei_fus.out,(flag.flight_mode+1),flag.unlock_sta);	
	}	
	else if(f.send_speed)
	{
    
		f.send_speed = 0;
		ANO_DT_Send_Speed(loc_ctrl_1.fb[Y],loc_ctrl_1.fb[X],loc_ctrl_1.fb[Z]);
	}
	else if(f.send_user)
	{
    
		f.send_user = 0;
		ANO_DT_Send_User();
		ANO_DT_Send_User1();
	}
	else if(f.send_senser)
	{
    
		f.send_senser = 0;
		ANO_DT_Send_Senser(sensor.Acc[X],sensor.Acc[Y],sensor.Acc[Z],sensor.Gyro[X],sensor.Gyro[Y],sensor.Gyro[Z],mag.val[X],mag.val[Y],mag.val[Z]);
	}	
	else if(f.send_senser2)
	{
    
		f.send_senser2 = 0;
		ANO_DT_Send_Senser2(baro_height,ref_tof_height,sensor.Tempreature_C*10);//原始数据
	}	
	else if(flag_send_omv)
	{
    
		flag_send_omv = 0;
		if(f.send_omv_ct)
		{
    
			f.send_omv_ct = 0;
			ANO_DT_SendOmvCt(opmv.cb.color_flag,opmv.cb.sta,opmv.cb.pos_x,opmv.cb.pos_y,opmv.cb.dT_ms);
		}
		else if(f.send_omv_lt)
		{
    
			f.send_omv_lt = 0;
			ANO_DT_SendOmvLt(opmv.lt.sta, opmv.lt.angle, opmv.lt.deviation, opmv.lt.p_flag, opmv.lt.pos_x, opmv.lt.pos_y, opmv.lt.dT_ms);
		}
	}
	else if(f.send_rcdata)
	{
    
		f.send_rcdata = 0;
		s16 CH_GCS[CH_NUM];
		
		for(u8 i=0;i<CH_NUM;i++)
		{
    
			if((chn_en_bit & (1<<i)))//(Rc_Pwm_In[i]!=0 || Rc_Ppm_In[i] !=0 )//该通道有值
			{
    
				CH_GCS[i] = CH_N[i] + 1500;
			}
			else
			{
    
				CH_GCS[i] = 0;
			}
		}ANO_DT_Send_RCData(CH_GCS[2],CH_GCS[3],CH_GCS[0],CH_GCS[1],CH_GCS[4],CH_GCS[5],CH_GCS[6],CH_GCS[7],0,0);
	}	
	else if(f.send_motopwm)
	{
    
		f.send_motopwm = 0;
#if MOTORSNUM == 8
		ANO_DT_Send_MotoPWM(motor[0],motor[1],motor[2],motor[3],motor[4],motor[5],motor[6],motor[7]);
#elif MOTORSNUM == 6
		ANO_DT_Send_MotoPWM(motor[0],motor[1],motor[2],motor[3],motor[4],motor[5],0,0);
#elif MOTORSNUM == 4
		ANO_DT_Send_MotoPWM(motor[0],motor[1],motor[2],motor[3],0,0,0,0);
#else
		
#endif
	}	
	else if(f.send_power)
	{
    
		f.send_power = 0;
		ANO_DT_Send_Power(Plane_Votage*100,0);
	}
	else if(f.send_sensorsta)
	{
    
		f.send_sensorsta = 0;
		ANO_DT_SendSensorSta(switchs.of_flow_on ,switchs.gps_on,switchs.opmv_on,switchs.uwb_on,switchs.of_tof_on);
	}
	else if(f.send_location)
	{
    
		f.send_location = 0;
		ANO_DT_Send_Location(switchs.gps_on,Gps_information.satellite_num,(s32)Gps_information.longitude,(s32)Gps_information.latitude,123,456);
		
	}
	else if(f.send_vef)
	{
    
		ANO_DT_Send_VER();
		f.send_vef = 0;
	}
	ANO_DT_Data_Receive_Anl_Task();
}

系统控制输出：

姿态角速度环控制 Att_1level_Ctrl(2*1e-3f);

void Att_1level_Ctrl(float dT_s)
{
      // 改变控制参数 （最小控制周期内）
	ctrl_parameter_change_task();
	// 目标角速度赋值
		 for(u8 i = 0;i<3;i++)
		{
    
			att_1l_ct.exp_angular_velocity[i] = val_2[i].out;// val_2[i].out;//
		}
	
		att_1l_ct.exp_angular_velocity[ROL] = LIMIT(att_1l_ct.exp_angular_velocity[ROL],-MAX_ROLLING_SPEED,MAX_ROLLING_SPEED);
		att_1l_ct.exp_angular_velocity[PIT] = LIMIT(att_1l_ct.exp_angular_velocity[PIT],-MAX_ROLLING_SPEED,MAX_ROLLING_SPEED);


	/* 反馈角速度赋值 */
	att_1l_ct.fb_angular_velocity[ROL] = ( sensor.Gyro_deg[X] );
	att_1l_ct.fb_angular_velocity[PIT] = (-sensor.Gyro_deg[Y] );
	att_1l_ct.fb_angular_velocity[YAW] = (-sensor.Gyro_deg[Z] );
	

	/* PID计算 */									 
 for(u8 i = 0;i<3;i++)
 {
    
		PID_calculate( dT_s,            //周期 （单位：秒）
										0,	//前馈值
										att_1l_ct.exp_angular_velocity[i],		//期待值（设定值）
										att_1l_ct.fb_angular_velocity[i],		//反馈值
										&arg_1[i], //PID参数结构体
										&val_1[i],	//PIDÊý¾Ý½á¹¹Ìå
                    200,//»ý·ÖÎó²îÏÞ·ù
										CTRL_1_INTE_LIM *flag.taking_off			//integration limit£¬»ý·Ö·ù¶ÈÏÞ·ù
										 )	; 
 
	
	 ct_val[i] = (val_1[i].out);
 }
										 

	/*¸³Öµ£¬×îÖÕ±ÈÀýµ÷½Ú*/
	mc.ct_val_rol =                   FINAL_P *ct_val[ROL];
	mc.ct_val_pit = X_PROPORTION_X_Y *FINAL_P *ct_val[PIT];
	mc.ct_val_yaw =                   FINAL_P *ct_val[YAW];
	/*Êä³öÁ¿ÏÞ·ù*/
	mc.ct_val_rol = LIMIT(mc.ct_val_rol,-1000,1000);
	mc.ct_val_pit = LIMIT(mc.ct_val_pit,-1000,1000);
	mc.ct_val_yaw = LIMIT(mc.ct_val_yaw,-400,400);	
}

_rolling_flag_st rolling_flag;

姿态角度环控制 Att_2level_Ctrl(6e-3f,CH_N);

电机输出控制 Motor_Ctrl_Task(2);

任务处理及控制：

线程任务执行函数： Main_Task

遥控器数据处理 RC_duty_task(11);

飞行模式设置任务 Flight_Mode_Set(11);

高度数据融合任务 (一)WCZ_Fus_Task(11);

高度数据融合任务 (二) GPS_Data_Processing_Task(11);

高度速度环控 Alt_1level_Ctrl(11e-3f);

高度环控制（一） Alt_2level_Ctrl(11e-3f)

高度环控制（二） AnoOF_DataAnl_Task(11);

灯光控制 LED_Task2(11);

罗盘数据处理任务 Mag_Update_Task(20);

程序指令控制 （一） ANO_OFDF_Task(20);

程序指令控制 （二） FlyCtrl_Task(20);

程序指令控制 （三） Ano_UWB_Data_Calcu_Task(20);

位置速度环控制 Loc_1level_Ctrl(20,CH_N);

电压： Power_UpdateTask(50);

恒温控制 : Thermostatic_Ctrl_Task(50);

延时存储 : Ano_Parame_Write_task(50);

用户自定义：

数据处理及传递 （解螺旋）MV_Decoupling(20)