[ongoing update...] 2021 National Electronic Design Competition for college students (III) interpretation of the anonymous four axis space developer flight control system design

Module equipment drawing ：

The overview ：

Interface function diagram （ One ）

Module function diagram （ Two ） positive

Module function diagram （ 3、 ... and ） Back

Chip schematic diagram :

Overall analysis and analysis

Take one key takeoff and altitude setting as an example ：

Header Directory

#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"

Light flicker function ： INT_1ms_Task()

void INT_1ms_Task()
{
    	
// if(fc_sta.start_ok == 0) return;
	
	// Mark 1ms perform 
	lt0_run_flag ++;
	// Light driven 
	LED_1ms_DRV();

	// Cycle count 
	circle_cnt[0] ++;
	//20 Secondary cycle 
	circle_cnt[0] %= CIRCLE_NUM;
	//
	if(!circle_cnt[0])
	{
    
	}
}

All sensor read functions ： Loop_Task_0()

static void Loop_Task_0()//1ms Do it once 
{
    
	// 
	/* Sensor data reading */
	Fc_Sensor_Get();
	/* Inertial sensor data preparation */
	Sensor_Data_Prepare(1);
	
	/* Attitude solution update */
	IMU_Update_Task(1);
	
	/* obtain WC_Z The acceleration */
	WCZ_Acc_Get_Task();
	WCXY_Acc_Get_Task();
	
	/* Flight status mission */
	Flight_State_Task(1,CH_N);
	
	/* Switch status task */
	Swtich_State_Task(1);
	
	/* Optical flow fusion data preparation task */
	ANO_OF_Data_Prepare_Task(0.001f);

	/* Data exchange */
	ANO_DT_Data_Exchange();	
}

Attitude ring and motor initialization ：Loop_Task_1

static void Loop_Task_1(u32 dT_us)	//2ms Do it once 
{
    
// 
	float t1_dT_s;
	t1_dT_s = (float)dT_us *1e-6f;
	//========================
	/* Attitude angular velocity loop control */
	Att_1level_Ctrl(2*1e-3f);
	
	/* Motor output control */
	Motor_Ctrl_Task(2);	

// 
}

Attitude loop angle control

static void Loop_Task_2(u32 dT_us)	//6ms Do it once 
{
    
// 
	float t2_dT_s;
	t2_dT_s = (float)dT_us *1e-6f;
	//========================
	/* Obtain attitude angle （ROLL PITCH YAW）*/
	calculate_RPY();
	
   User_my_yaw_2level(6,line);	// Line finding YAW correct 
	/* Attitude angle loop control */
	Att_2level_Ctrl(6e-3f,CH_N);

	//
	
// 
}

Mission settings ：void Loop_Task_5

static void Loop_Task_5(u32 dT_us)	//11ms Do it once 
{
    	
// 
	float t2_dT_s = (float)dT_us *1e-6f;//0.008f;//
	//========================
	/* Remote control data processing */
	RC_duty_task(11);
	
	/* Flight mode setting task */
	Flight_Mode_Set(11);
	

	
	/* High data fusion task */
	WCZ_Fus_Task(11);
	GPS_Data_Processing_Task(11);
	
	/* Height speed loop control */
	Alt_1level_Ctrl(11e-3f);
	
	/* Height ring control */
	Alt_2level_Ctrl(11e-3f);
	
	/*--*/	
	AnoOF_DataAnl_Task(11);

	/* Light control */	
	LED_Task2(11);


// 
}

Mission specific functions ：Loop_Task_8

extern struct _MV_ 		MV;

static void Loop_Task_8(u32 dT_us)	//20ms Do it once 
{
    
	u8 dT_ms = 20;//(u8)(dT_us *1e-3f);
	//==========================
	//
	/* Compass data processing task */
	Mag_Update_Task(20);
	/* Program command control */
	FlyCtrl_Task(20);
	//
	ANO_OFDF_Task(20);
	/*--*/
	Ano_UWB_Data_Calcu_Task(20);
	/* Position speed loop control */
	Loc_1level_Ctrl(20,CH_N);
	
	
	/* user */
	MV_Decoupling(20);  // Transfer the data processing to the aircraft   Unrotation 
	Loc_2level_Ctrl(20,&MV);
	Tracking_Ctrlw(0.02f);      // The procedure of small aircraft modification 
// Tracking_Ctrl(0.02f); // Frame wrapping 
	
// Proce0_Ctrl(0.02f); // Fixed point 

      Anonymous programs  
// /*OPMV Check whether the line drops */
// OpenMV_Offline_Check(20);
// /*OPMV Color block tracking data processing task */
// ANO_CBTracking_Task(20);
// /*OPMV Line finding data processing task */
// ANO_LTracking_Task(20);
// /*OPMV Control the mission */
// ANO_OPMV_Ctrl_Task(20);

}

Voltage and temperature control letters ：Loop_Task_9

static void Loop_Task_9(u32 dT_us)	//50ms Do it once 
{
    
	//
	/* Voltage related tasks */
	Power_UpdateTask(50);
	// Thermostatic control （ Do not comment out directly , Otherwise, the machine will not pass the calibration ）
	Thermostatic_Ctrl_Task(50);
	// /* Delay storage tasks */
	Ano_Parame_Write_task(50);
}

System task multithread configuration function ：

static sched_task_t sched_tasks[] = 
{
    
	// Mission n,  cycle us,  Last time 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 },
};

Thread execution function

u8 Main_Task(void)
{
    
	uint8_t index = 0;
	
	// Inquire about 1ms Whether the task needs to be performed 
	if(lt0_run_flag!=0)
	{
    
		//
		lt0_run_flag--;
		Loop_Task_0();
	}
	// Loop through all other thread tasks , Whether to execute 
	uint32_t time_now,delta_time_us;
	for(index=0;index < TASK_NUM;index++)
	{
    
		// Get the current time of the system , Company US
		 time_now = GetSysRunTimeUs();//SysTick_GetTick();
		// Judge , If the current time minus the last execution time , Is greater than or equal to the execution cycle of the thread , Execute thread 
		if(time_now - sched_tasks[index].last_run >= sched_tasks[index].interval_ticks)
		{
    
			delta_time_us = (u32)(time_now - sched_tasks[index].last_run);

			// Update thread execution time , For next judgment 
			sched_tasks[index].last_run = time_now;
			// Executing thread functions , Using function pointers 
			sched_tasks[index].task_func(delta_time_us);

		}	 
	}
	
	return 0;
}

Initialization function structure analysis ：

LED initialization ： Dvr_LedInit(void)

analysis ： You can see that the initialization function consists of two parts
- The first part ：ROM_SysCtlPeripheralEnable （GPIO mouth ）
- The second part ：ROM_GPIOPinTypeGPIOOutput（GPIO mouth ,GPIO Pin ）

#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);
}

Sensor data reading function

analysis ： Flight control three sensor data

• gyroscope The acceleration
• Electronic compass
• barometer
• These functions are well encapsulated In order to facilitate us to parse the program

void Fc_Sensor_Get()//1ms
{
    
	static u8 cnt;
	if(flag.start_ok)
	{
    
		Drv_Icm20602_Read(); // gyroscope   Accelerometer 
		
		cnt ++;
		cnt %= 20;
		if(cnt==0)
		{
    
			Drv_AK8975_Read(); //  Electronic compass magnetometer data 
			baro_height = (s32)Drv_Spl0601_Read(); // Read the barometer data 
		}
	}	
	test_time_cnt++;

}

Gyro acceleration extraction ：Drv_Icm20602_Read

void Drv_Icm20602_Read( void )
{
    
	// Read the sensor register , Read continuously 14 Bytes 
	icm20602_readbuf(MPUREG_ACCEL_XOUT_H,14,mpu_buffer);
	// Data assignment 
	ICM_Get_Data();
}

Electronic compass magnetometer 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();
	
}

Read the barometer data ：(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;
}

Inertial sensor detection function

Static detection function ：motionless_check(dT_ms);

analysis : By judgment T To determine whether it is at rest
and T The standard of judgment is Subtract the old angle data from the original data

void Sensor_Basic_Init()
{
    
  #  Re offset from the sensor 
	Center_Pos_Set();
	
	sensor.acc_z_auto_CALIBRATE = 1; #  Start up automatic calibration and alignment Z Axis 
	sensor.gyr_CALIBRATE = 2;  #  Start up and calibrate the gyroscope automatically 
}
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;
	}

}

gyroscope MPU6050 function 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();
				}	
			}
		}
	}
}

Inertial sensors convert units Function segment

	for(u8 i =0 ;i<3;i++)
		{
    
		 #  Gyroscope switches to degrees per second   range +-2000 degree 
			sensor.Gyro_deg[i] = sensor.Gyro[i] *0.061036f ;
		#  The gyroscope converts to radians per second   range +-2000 degree 
			sensor.Gyro_rad[i] = sensor.Gyro_deg[i] *0.01745f;//sensor.Gyro[i] 
		#  Accelerometers convert to centimeters   Per square second   range  +- 8G
			sensor.Acc_cmss[i] = (sensor.Acc[i] *RANGE_PN16G_TO_CMSS );// /65535 * 16*981; +-8G 
		}

Attitude solution update function IMU_Update_Task(1);

effect ： Gravity sensors as well as Magnetometer Perform attitude calculation
The notes are as follows ： If you are ready to fly Reset the gravity mark and magnetometer reset mark
Calibrate the gyroscope Do not save
Automatic reset
The reset flag has been set
Set gravity complementary fusion correction Kp coefficient
Set gravity complementary fusion correction ki coefficient
Set compass complementary fusion correction ki coefficient
Magnetometer correction enable
Attitude calculation to update The fusion

void IMU_Update_Task(u8 dT_ms)
{
    
//  If you are ready to fly   Reset the gravity mark and magnetometer reset mark  
   			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;// Automatic reset 
   					//sensor.gyr_CALIBRATE = 2;
   				}	
   				
   				if(reset_imu_f==0 )//&& flag.motionless == 1)
   				{
    
   					imu_state.G_reset = 1;// Automatic reset 
   					sensor.gyr_CALIBRATE = 2;// Calibrate the gyroscope   Do not save 
   					reset_imu_f = 1;     // The reset flag has been set 
   				}
   			}				
   			if(0) 
   			{
    
   				imu_state.gkp = 0.0f;
   				imu_state.gki = 0.0f;
   			}
   			else
   			{
    
   				if(0)
   				{
    
   					imu_state.gkp = 0.2f;
   				}
   				else
   				{
    
   					// Set gravity complementary fusion correction Kp coefficient 
   					imu_state.gkp = 0.2f;//0.4f;
   				}
   				//  Set gravity complementary fusion correction ki coefficient 
   				imu_state.gki = 0.01f;
   			//  Set compass complementary fusion correction ki coefficient 
   				imu_state.mkp = 0.1f;
   			}
   			imu_state.M_fix_en = sens_hd_check.mag_ok;		// Magnetometer correction enable 
   			// Attitude calculation   to update   The fusion 
   			IMU_update(dT_ms *1e-3f, &imu_state,sensor.Gyro_rad, sensor.Acc_cmss, mag.val,&imu_data);//x3_dT_1[2] * 0.000001f
}

Get the acceleration function ( One )WCZ_Acc_Get_Task();

get Z The acceleration on the axis
notes ： Get the minimum period

void WCZ_Acc_Get_Task()// Minimum period 
{
    
	wcz_acc_use += 0.03f *(imu_data.w_acc[Z] - wcz_acc_use);
}

Get the acceleration function ( Two )WCXY_Acc_Get_Task();

get X Y The acceleration on the axis
notes ： Minimum period

void WCXY_Acc_Get_Task(void)// Minimum period 
{
    
	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);
}

Task scheduling ：

Flight status mission ：Flight_State_Task(1,CH_N)

Flight status task scheduling ：
The notes are as follows ：

• Set the throttle rocker amount
• Push the oil valve to start
• take off 1 second after I think I'm already flying
• Set up Speed of rise
• Set up Descent speed
• Flight control system z Speed target scalar comprehensive setting
• Speed setting amount Plus or minus Reference resources ANO Coordinate reference direction
• Flight control system XY Speed Target comprehensive quantity setting
• Call the function to detect landing
• Tilt too big lock

void Flight_State_Task(u8 dT_ms,s16 *CH_N)
{
    
	s16 thr_deadzone;
	static float max_speed_lim,vel_z_tmp[2];
	//  Set the throttle rocker amount 
	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]);
	//  Push the oil valve to start 
	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
		{
    
	//  take off 1 second   after   I think I'm already flying 
			flag.flying = 1;  
		}
		
		if(fs.speed_set_h_norm[Z]>0)
		{
    
  //  Set up   Speed of rise  
			vel_z_tmp[0] = (fs.speed_set_h_norm_lpf[Z] *MAX_Z_SPEED_UP);
		}
		else
		{
    
 //  Set up   Descent speed 
			vel_z_tmp[0] = (fs.speed_set_h_norm_lpf[Z] *MAX_Z_SPEED_DW);
		}
 //  Flight control system  z Speed target scalar comprehensive setting 
		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];
//  Speed setting amount   Plus or minus   Reference resources ANO  Coordinate reference direction 
	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;
	
	// Flight control system  XY Speed   Target comprehensive quantity setting 
	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];	
	
//  Call the function to detect landing 
	land_discriminat(dT_ms);
	
//  Tilt too big lock 
	if(rolling_flag.rolling_step == ROLL_END)
	{
    
	if(imu_data.z_vec[Z<0.25f)  //75 degree 
	// Tilt over   Lock it up 
		{
    
			//
			if(mag.mag_CALIBRATE==0)
			{
    
				imu_state.G_reset = 1;
			}
			flag.unlock_cmd = 0;
		}

	}	
		//
// Calibration in progress  , Reset the gravity direction 
	if(sensor.gyr_CALIBRATE != 0 || sensor.acc_CALIBRATE != 0 ||sensor.acc_z_auto_CALIBRATE)
	{
    
		imu_state.G_reset = 1;
	}
//  When resetting the direction of gravity , The sensor is considered to have failed 
	if(imu_state.G_reset == 1)
	{
    
		flag.sensor_imu_ok = 0;
		LED_STA.rst_imu = 1;
		WCZ_Data_Reset(); 
		// Reset   High data fusion 
	}
	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!");
		}
	}
	
	/* Flight status reset */
	if(flag.unlock_sta == 0)
	{
    
		flag.flying = 0;
		landing_cnt = 0;
		flag.taking_off = 0;
		flying_cnt = 0;	
		flag.rc_loss_back_home = 0;
		// Reset fusion 
		if(flag.taking_off == 0)
		{
    
// wxyz_fusion_reset();
		}
	}
	

}

Switch status task ： Swtich_State_Task(1);

Switch status task function ： Determine whether each task status is valid

• Optical flow module
• Optical flow quality Greater than 60*
• Optical flow quality Greater than 50*
• Or before flying
• Consider optical flow available . Determine the available delay time
• Optical flow height 600 cm Effective within
• Time delay 1.5s Judge whether the optical flow is effective
• Invalid decision height
• GPS
• UWB
• OPENMV

void Swtich_State_Task(u8 dT_ms)
{
    
	switchs.baro_on = 1;
	//  Optical flow module 
	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  Optical flow quality   Greater than  60* //  Optical flow quality   Greater than 50* /  Or before flying  //  Consider optical flow available . Determine the available delay time 
		{
    
			if(of_quality_delay<500)
			{
    
				of_quality_delay += dT_ms;
			}
			else
			{
    
				of_quality_ok = 1;
			}
		}
		else
		{
    
			of_quality_delay =0;
			of_quality_ok = 0;
		}
		
//  Optical flow height  600 cm Effective within 
		if(jsdata.of_alt<600)
		{
    
			//
			of_tof_on_tmp = 1;
			jsdata.valid_of_alt_cm = jsdata.of_alt;
//  Time delay  1.5s  Judge whether the optical flow is effective 
			if(of_alt_delay<1500)
			{
    
				of_alt_delay += dT_ms;			
			}
			else
			{
    
	//  Determine the height of validity 
				of_alt_ok = 1;
			}
		}
		else
		{
    
			// 
			of_tof_on_tmp = 0;
			//  Time delay 1.5 Seconds to determine whether the laser height is effective 
			if(of_alt_delay>0)
			{
    
				of_alt_delay -= dT_ms;	
			}
			else
			{
    
				// Invalid decision height 
				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;
	}
	}

Optical flow fusion data conversion task ： ANO_OF_Data_Prepare_Task(0.001f);

Function name ANO_OF_DATA_Check_Task
Functional specifications ： Optical flow preparation data task
Parameters Cycle time （s）
Return value nothing

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]);
}

Data transmission data exchange task ：ANO_DT_Data_Exchange();

• Function name ： ANO_DT_Data_Exchange();
• effect ： Data transmission data exchange task
• Tips ：Data_Exchange Function to handle various data sending requests , For example, we want to realize every 5ms Send the sensor data to the upper computer once , That is to say, it is implemented in this function
• Call duration ： This function should be used by the user every 1ms Call once

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);// Raw data 
	}	
	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 )// This channel has a value 
			{
    
				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();
}

System control output ：

Attitude angular velocity loop control Att_1level_Ctrl(2*1e-3f);

void Att_1level_Ctrl(float dT_s)
{
      //  Change the control parameters  （ Within the minimum control period ）
	ctrl_parameter_change_task();
	//  Target angular velocity assignment 
		 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);


	/*  Feedback angular velocity assignment  */
	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 Calculation  */									 
 for(u8 i = 0;i<3;i++)
 {
    
		PID_calculate( dT_s,            // cycle  （ Company ： second ）
										0,	// Feedforward value 
										att_1l_ct.exp_angular_velocity[i],		// Look forward to （ The set value ）
										att_1l_ct.fb_angular_velocity[i],		// Feedback value 
										&arg_1[i], //PID Parametric structure 
										&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;

Attitude angle loop control Att_2level_Ctrl(6e-3f,CH_N);

Motor output control Motor_Ctrl_Task(2);

Task processing and control ：

Thread task execution function ： Main_Task

Remote control data processing RC_duty_task(11);

Flight mode setting task Flight_Mode_Set(11);

High data fusion task ( One )WCZ_Fus_Task(11);

High data fusion task ( Two ) GPS_Data_Processing_Task(11);

High speed environmental control Alt_1level_Ctrl(11e-3f);

Height ring control （ One ） Alt_2level_Ctrl(11e-3f)

Height ring control （ Two ） AnoOF_DataAnl_Task(11);

Light control LED_Task2(11);

Compass data processing task Mag_Update_Task(20);

Program command control （ One ） ANO_OFDF_Task(20);

Program command control （ Two ） FlyCtrl_Task(20);

Program command control （ 3、 ... and ） Ano_UWB_Data_Calcu_Task(20);

Position speed loop control Loc_1level_Ctrl(20,CH_N);

voltage ： Power_UpdateTask(50);

Thermostatic control : Thermostatic_Ctrl_Task(50);

Delayed storage : Ano_Parame_Write_task(50);

User customization ：

Data processing and transmission （ Unwinding ）MV_Decoupling(20)