代码解读九 文件名“Ano_MagProcess.c”

本部分主要是关于磁力计进行校准操作的，用户手册上有详细步骤，有需要可以看看

#include "Ano_MagProcess.h"
#include "Drv_LED.h"


//本文件是关于磁力计校准及相关处理的，可以结合用户手册及灯光的变化进行磁力计校准等操作




static s16 max_t[VEC_XYZ];
static s16 min_t[VEC_XYZ];


_mag_cal_st mag;

//磁力计计算复位，算是初始值了
//mode -> 1/2/其他
static void Mag_Cal_Reset(u8 mode)
{
	if(mode == 2)
	{
		for(u8 i = 0;i<2;i++)
		{
			max_t[i] = 0;
			min_t[i] = 30000;

		}
	}
	else if(mode == 1)
	{
			max_t[Z] = 0;
			min_t[Z] = 30000;		
	}
	else
	{
		for(u8 i = 0;i<3;i++)
		{
			max_t[i] = 0;
			min_t[i] = 30000;

		}	
	}
}


//计算X/Y轴
//进行限幅
static void Mag_Cal_XY(s16 mag_in[])
{
	for(u8 i = 0;i<2;i++)
	{
		max_t[i] = _MAX(max_t[i],mag_in[i]);
		min_t[i] = _MIN(min_t[i],mag_in[i]);
	}
	
}


//计算Z轴
static void Mag_Cal_Z(s16 mag_in[])
{
	max_t[Z] = _MAX(max_t[Z],mag_in[Z]);
	min_t[Z] = _MIN(min_t[Z],mag_in[Z]);
}

static u8 mag_cal_step;


//磁力计数据处理函数
void Mag_Data_Deal_Task(u8 dT_ms,s16 mag_in[],float z_vec_z,float gyro_deg_x,float gyro_deg_z)
{	
	static u16 cali_cnt;
	static float mag_cal_angle[2];
	float t_length;
	
	//就这个循环比较重要，得出mag.val[i]即三个方向的磁力计的值用于后面进行校准
	for(u8 i = 0;i<3;i++)
	{
		save.mag_gain[i] = LIMIT(save.mag_gain[i],0.05f,100);
		mag.val[i] = (mag_in[i] - save.mag_offset[i]) *save.mag_gain[i];
	}

	
	


	
//以下都是关于磁力计校准的操作，相当于一个状态机,按着要求来就行，具体操作见用户手册
//
/***************************************************************************************
****************************************************************************************
***************************************************************************************/
	if(mag.mag_CALIBRATE!= 0)
	{	
		switch(mag_cal_step)
		{
			case 0://第一步，水平旋转

				Mag_Cal_XY(mag_in);			
			
				if(z_vec_z<0.985f)//+-10deg
				{
					LED_state = 16;
					mag_cal_step = 1;
					mag_cal_angle[0] = 0;
				}
				else
				{	
					LED_state = 5;
					mag_cal_angle[0] += dT_ms *1e-3f *(gyro_deg_z); //角度积分，旋转360度
					if(ABS(mag_cal_angle[0])>360)
					{
						mag_cal_angle[0] = 0;
						mag_cal_step = 2;
					}
				}
			break;
			
			case 1://error
				
				Mag_Cal_Reset(2);
				mag_cal_angle[0] = 0;
				mag_cal_step = 0;
			break;
			
			case 2://第二步，竖直旋转，机头朝下
				LED_state = 6;
				if(z_vec_z<0.1f)//5.7deg
				{
					mag_cal_step = 3;
				}
			break;
			
			case 3:
				mag.mag_CALIBRATE = 2;																					
				
				Mag_Cal_Z(mag_in);

				if(z_vec_z>0.17f)//10deg
				{
					LED_state = 6;
					mag_cal_step = 4;
					mag_cal_angle[1] = 0;
				}
				else
				{
					LED_state = 7;
					mag_cal_angle[1] += dT_ms *1e-3f *(gyro_deg_x);	//角度积分，旋转360度
					if(ABS(mag_cal_angle[1])>360)
					{
						mag_cal_angle[1] = 0;
						mag_cal_step = 5;
					}
				}			
			break;
			
			case 4://error_2，重新开始竖直旋转
				Mag_Cal_Reset(1);
				mag_cal_angle[1] = 0;
				mag_cal_step = 2;				
			break;
			
			case 5:
				for(u8 i = 0;i<3;i++)
				{
					save.mag_offset[i] = 0.5f *(max_t[i] + min_t[i]);		//中值校准
					save.mag_gain[i] = safe_div(200.0f ,(0.5f *(max_t[i] - min_t[i])),0);		//幅值校准
				}
				
				Mag_Cal_Reset(3);		
				mag_cal_angle[0] = mag_cal_angle[1] = 0;		
				mag_cal_step = 0;
				mag.mag_CALIBRATE = 0;			
				
				data_save();//保存数据
						
				LED_state = 0;
			break;
			
			default:break;	
		}
		
		
		if(mag_cal_step == 0 || mag_cal_step == 3)
		{
			//长时间出错，退出校准逻辑
			if(cali_cnt<15000)
			{
				cali_cnt+= dT_ms;
				
			}
			else校准错误
			{
				LED_state = 8;
				mag.mag_CALIBRATE = 0;
				
			}
		}
		else
		{
			cali_cnt = 0;
		}
	}
	else
	{
														//if(LED_state ==4 || LED_state ==5) LED_state = 0;
		
		mag_cal_step = 0;

		
		
		t_length = my_3_norm(mag.val[X],mag.val[Y],mag.val[Z]);
		
		if(t_length<150||t_length>350)
		{
			//state[3] |= (1<<3);//罗盘严重干扰
																											//LED_state = 6;
		}
		else
		{
			//state[3] &= ~(1<<3);//罗盘无严重干扰
			
//			if(LED_state == 6)
//			{
//				LED_state = 0;
//			}
		}
///	
	}
}