代码解读十 文件名“Ano_FlightDataCal.c”

本部分主要是对IMU测量模块测量的值进行后续处理，同时在飞行过程中不断对数据进行更新，然后进行姿态解算，便于后续丢进PID中进行进一步处理。根据所处位置及函数调用情况不难发现此部分算是对底层的进一步封装，便于在任务调度器中进行调用

#include "Ano_FlightDataCal.h"
#include "Ano_Imu.h"
#include "Drv_icm20602.h"
#include "Ano_MagProcess.h"
#include "Drv_spl06.h"
#include "Drv_ak8975.h"
#include "Ano_MotionCal.h"
#include "Drv_vl53l0x.h"
#include "Drv_led.h"
#include "Ano_OF.h"









u16 test_time_cnt;

//传感器数据获取函数
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++;

}

extern s32 sensor_val_ref[];

static u8 reset_imu_f;

//传感器更新函数
void IMU_Update_Task(u8 dT_ms)
{


	
		
			/*如果准备飞行，复位重力复位标记和磁力计复位标记*/
				if(flag.fly_ready )
				{
					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.3f;//0.4f;
					}
					
					/*设置重力互补融合修正ki系数*/
					imu_state.gki = 0.002f;
					
					/*设置罗盘互补融合修正ki系数*/
					imu_state.mkp = 0.2f;
				}
				
				imu_state.M_fix_en = sens_hd_check.mag_ok;		//磁力计修正使能
				
//				imu_state.obs_en = 1;
//				imu_data.obs_acc_w[X] = flow_decoupling.w_ref_acc[X];
//				imu_data.obs_acc_w[Y] = flow_decoupling.w_ref_acc[Y];

				//重要
				//调用姿态解算函数更新姿态/
				IMU_update(dT_ms *1e-3f, &imu_state,sensor.Gyro_rad, sensor.Acc_cmss, mag.val,&imu_data);//x3_dT_1[2] * 0.000001f
//	
}

static s16 mag_val[3];


//磁力计更新函数
//要和Ano_MagProcess.c结合起来
void Mag_Update_Task(u8 dT_ms)
{
	//获取磁力计的值
	Mag_Get(mag_val);
	
	//调用磁力计数据处理函数
	Mag_Data_Deal_Task(dT_ms,mag_val,imu_data.z_vec[Z],sensor.Gyro_deg[X],sensor.Gyro_deg[Z]);
	
}


s32 baro_height,baro_h_offset,ref_height_get_1,ref_height_get_2,ref_height_used;
s32 baro2tof_offset,tof2baro_offset;

float baro_fix1,baro_fix2,baro_fix;

static u8 wcz_f_pause;
float wcz_acc_use;			


//这个WCZ简写的至今没明白是什么意思，但是肯定和高度环有关，
//应该是气压计和激光有关的，是对其数据进行获取或者处理吧，
//简单了解下

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

//void Baro_Get_Task()
//{
			ref_height_get += LIMIT((s32)user_spl0601_get() - ref_height_get,-20,20 );
	baro_height =(s32)user_spl0601_get();
//}

u16 ref_tof_height;
static u8 baro_offset_ok,tof_offset_ok;


void WCZ_Fus_Task(u8 dT_ms)
{

	
	if(flag.taking_off)
	{
		baro_offset_ok = 2;
	}
	else
	{
		if(baro_offset_ok == 2)
		{
			baro_offset_ok = 0;
		}
	}
	
	if(baro_offset_ok >= 1)//(flag.taking_off)
	{
		ref_height_get_1 = baro_height - baro_h_offset + baro_fix  + tof2baro_offset;//气压计相对高度，切换点跟随TOF
		//baro_offset_ok = 0;
	}
	else
	{
		if(baro_offset_ok == 0 )
		{
			baro_h_offset = baro_height;
			if(flag.sensor_ok)
			{
				baro_offset_ok = 1;
			}
		}
	}
	
	if((flag.flying == 0) && flag.auto_take_off_land == AUTO_TAKE_OFF	)
	{
		wcz_f_pause = 1;
		
		baro_fix = 0;
	}
	else
	{
		wcz_f_pause = 0;
		
		if(flag.taking_off == 0)
		{
			baro_fix1 = 0;
			baro_fix2 = 0;
				
		}
		baro_fix2 = -BARO_FIX;

		
		baro_fix = baro_fix1 + baro_fix2 - BARO_FIX;//+ baro_fix3;
	}
	
	
	if((sens_hd_check.tof_ok || sens_hd_check.of_ok) && baro_offset_ok) //TOF或者OF硬件正常，且气压计记录相对值以后
	{
		if(switchs.tof_on || switchs.of_tof_on) 													//TOF数据有效
		{
			if(switchs.of_tof_on) 																					//光流带TOF，光流优先
			{
				ref_tof_height = OF_ALT;
			}
			else
			{
				ref_tof_height = tof_height_mm/10;
			}
			
			
			if(tof_offset_ok == 1)
			{
				ref_height_get_2 = ref_tof_height + baro2tof_offset;					//TOF参考高度，切换点跟随气压计
				
				ref_height_used = ref_height_get_2;
				
				tof2baro_offset += 0.5f *((ref_height_get_2 - ref_height_get_1) - tof2baro_offset);//记录气压计切换点，气压计波动大，稍微滤波一下
				//tof2baro_offset = ref_height_get_2 - ref_height_get_1;				
			}
			else
			{
				baro2tof_offset = ref_height_get_1 - ref_tof_height ; 				//记录TOF切换点
				
				tof_offset_ok = 1;
				
				
			}
			

		}
		else
		{
			
			tof_offset_ok = 0;
			
			ref_height_used = ref_height_get_1 ;
		}
	}
	else
	{
		ref_height_used = ref_height_get_1;
	}
	
	WCZ_Data_Calc(dT_ms,wcz_f_pause,(s32)wcz_acc_use,(s32)(ref_height_used));

}