匿名四轴【 任务一(1000Hz)惯性传感器数据读取一】

上一篇说了传感器数据读取，接下来来看惯性传感器数据读取

//我们来看这个函数现在到了第二部分惯性传感器数据读取
u32 test_dT_1000hz[3],test_rT[6];
static void Loop_1000Hz(void)	//1ms执行一次
{
	test_dT_1000hz[0] = test_dT_1000hz[1];
	test_rT[3] = test_dT_1000hz[1] = GetSysTime_us ();
    //记录当前的时间，并将存储的时间存储在test_rT[3]，
    //test_dT_1000hz[1]中
    //这样我们再看上面就是把上次存储的时间存储在test_dT_1000hz[0]
    //总的来说
    //test_dT_1000hz[0]上一次时间
    //test_dT_1000hz[1]这次的时间
	test_dT_1000hz[2] = (u32)(test_dT_1000hz[1] - test_dT_1000hz[0]) ;
    //test_dT_1000hz[2]记录这次时间与上一次时间的差值
    

	/*传感器数据读取*/
	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]) ;	

}

我们来看具体的传感器数据准备

//惯性传感器数据准备
void Sensor_Data_Prepare(u8 dT_ms)
//在上面函数中这边的dT_ms为1，
//还记得我们前面谈到的1000Hz是1ms执行一次，
//这边的dT_ms指的应该就是两次之间相隔的时间，
{	
 	//定义频率，如果前后执行任务之间间距是1ms则可以看到频率为1000HZ
	float hz = 0 ;
	if(dT_ms != 0) hz = 1000/dT_ms;//判断dT_ms是否为0，否则不能作为除数，避免错误
    
//	MPU6050_Read();
//	sensor_rotate_func(dT);
	
	/*静止检测*/
	motionless_check(dT_ms);
    //在后面我们会讲到，可以先去看一下下面的讲解，再回来看剩下的代码
			
	MPU6050_Data_Offset(); //校准函数


	/*得出校准后的数据*/
	for(u8 i=0;i<3;i++)
	{ 
		
		sensor_val[A_X+i] = sensor.Acc_Original[i] ;

		sensor_val[G_X+i] = sensor.Gyro_Original[i] - save.gyro_offset[i] ;
		//sensor_val[G_X+i] = (sensor_val[G_X+i] >>2) <<2;
	}
	
	/*可将整个传感器坐标进行旋转*/
//	for(u8 j=0;j<3;j++)
//	{
//		float t = 0;
//		
//		for(u8 i=0;i<3;i++)
//		{
//			
//			t += sensor_val[A_X + i] *wh_matrix[j][i]; 
//		}
//		
//		sensor_val_rot[A_X + j] = t;
//	}

//	for(u8 j=0;j<3;j++)
//	{
//		float t = 0;
//		
//		for(u8 i=0;i<3;i++)
//		{
//			
//			t += sensor_val[G_X + i] *wh_matrix[j][i]; 
//		}
//		
//		sensor_val_rot[G_X + j] = t;
//	}	

	/*赋值*/
	for(u8 i = 0;i<6;i++)
	{
		sensor_val_rot[i] = sensor_val[i];
	}

	/*数据坐标转90度*/
	sensor_val_ref[G_X] =  sensor_val_rot[G_Y] ;
	sensor_val_ref[G_Y] = -sensor_val_rot[G_X] ;
	sensor_val_ref[G_Z] =  sensor_val_rot[G_Z];

	
	sensor_val_ref[A_X] =  (sensor_val_rot[A_Y] - save.acc_offset[Y] ) ;
	sensor_val_ref[A_Y] = -(sensor_val_rot[A_X] - save.acc_offset[X] ) ;
	sensor_val_ref[A_Z] =  (sensor_val_rot[A_Z] - save.acc_offset[Z] ) ;
	
	/*单独校准z轴模长*/
	mpu_auto_az();

//======================================================================
	
	/*软件低通滤波*/
	for(u8 i=0;i<3;i++)
	{	
		//
		gyr_f[4][X +i] = (sensor_val_ref[G_X + i] );
		acc_f[4][X +i] = (sensor_val_ref[A_X + i] );
		//
		for(u8 j=4;j>0;j--)
		{
			//
			gyr_f[j-1][X +i] += GYR_ACC_FILTER *(gyr_f[j][X +i] - gyr_f[j-1][X +i]);
			acc_f[j-1][X +i] += GYR_ACC_FILTER *(acc_f[j][X +i] - acc_f[j-1][X +i]);
		}
		
//		LPF_1_(100,dT_ms*1e-3f,sensor_val_ref[G_X + i],sensor.Gyro[X +i]);
//		LPF_1_(100,dT_ms*1e-3f,sensor_val_ref[A_X + i],sensor.Acc[X +i]);
				
	}
	
			/*旋转加速度补偿*/
//======================================================================
	
	for(u8 i=0;i<3;i++)
	{	
		center_pos.gyro_rad_old[i] = center_pos.gyro_rad[i];
		center_pos.gyro_rad[i] =  gyr_f[0][X + i] *RANGE_PN2000_TO_RAD;//0.001065f;
		center_pos.gyro_rad_acc[i] = hz *(center_pos.gyro_rad[i] - center_pos.gyro_rad_old[i]);
	}
	
	center_pos.linear_acc[X] = +center_pos.gyro_rad_acc[Z] *center_pos.center_pos_cm[Y] - center_pos.gyro_rad_acc[Y] *center_pos.center_pos_cm[Z];
	center_pos.linear_acc[Y] = -center_pos.gyro_rad_acc[Z] *center_pos.center_pos_cm[X] + center_pos.gyro_rad_acc[X] *center_pos.center_pos_cm[Z];
	center_pos.linear_acc[Z] = +center_pos.gyro_rad_acc[Y] *center_pos.center_pos_cm[X] - center_pos.gyro_rad_acc[X] *center_pos.center_pos_cm[Y];
	
//======================================================================
	/*赋值*/
	for(u8 i=0;i<3;i++)
	{

		
		sensor.Gyro[X+i] = gyr_f[0][i];//sensor_val_ref[G_X + i];
		
		sensor.Acc[X+i] = acc_f[0][i] - center_pos.linear_acc[i]/RANGE_PN16G_TO_CMSS;//sensor_val_ref[A_X+i];//
	}
	
	/*转换单位*/
		for(u8 i =0 ;i<3;i++)
		{
			/*陀螺仪转换到度每秒，量程+-2000度*/
			sensor.Gyro_deg[i] = sensor.Gyro[i] *0.061036f ;//  /65535 * 4000; +-2000度 0.061

			/*陀螺仪转换到弧度度每秒，量程+-2000度*/
			sensor.Gyro_rad[i] = sensor.Gyro_deg[i] *0.01745f;//sensor.Gyro[i] *RANGE_PN2000_TO_RAD ;//  0.001065264436f //微调值 0.0010652f
		
			/*加速度计转换到厘米每平方秒，量程+-8G*/
			sensor.Acc_cmss[i] = (sensor.Acc[i] *RANGE_PN16G_TO_CMSS );//   /65535 * 16*981; +-8G
		
		}

}

motionless_check函数

//静止检测，motionless英文的意思就是静止的，不运动的
enum
{
	X = 0,
	Y = 1,
	Z = 2,
	VEC_XYZ,
    //这里放一个数是为了说明这个枚举已经有3个值了，所以VEC为3
};
//首先我们看一下这个枚举，如果看不懂建议先去看一下我前面写的
//这里是一个枚举类型前面有讲过，这里的VEC_XYZ为3，因为枚举递增
float g_d_sum[VEC_XYZ] = {500,500,500};
s16 g_old[VEC_XYZ];//s16就是int16_t
//这边我们还看不出这个变量有什么用，
//不过关于这个变量的运用基本上就是下面这个函数
//所以只要看懂下面这个函数，就可以知道这个变量是用来干什么的
void motionless_check(u8 dT_ms)
{
	u8 t = 0;
	for(u8 i = 0;i<3;i++)
	{
     //#define ABS(x) ( (x)>0?(x):-(x) )----ABS就是取绝对值
		g_d_sum[i] += 3*ABS(sensor.Gyro_Original[i] - g_old[i]) ;
	//看到sensor.Gyro_Original[i] 不要慌，看一下我们前面有没讲过
    //果然在前面读取传感器数据我们曾讲过这个东西，来看一下代码
    /*
        这里的Gyro应该是角速度
        sensor.Gyro_Original[X] = temp[1][X];
        sensor.Gyro_Original[Y] = temp[1][Y];
        sensor.Gyro_Original[Z] = temp[1][Z];
    */
    //而前面我们分析过temp中记录中的20602传感器的数据，
    //就相当于Gyro_Original存储的是从20602读出的角速度数据
    //那么现在这行代码这剩下g_old没解决，g_old引用地方只有下面那句话
    //g_old[i] = sensor.Gyro_Original[i];
    //很显然了存储的也是角速度，那只有可能是过去的角速度，
    //想象一下，我们上面提到的几行代码是在下面哪一行代码执行
    //的那么我们下次执行到这里时那么Gyro_Original的值已经更新了，
    //但是g_old存储的还是原来的，
    //那这里g_de_sum存储的就是原来的旧的数据
    //那么这行代码的意思就是（这次的值与上一次值的偏差）的绝对值*3，
    //这边*3是什么意思就是放大3倍的意思
		g_d_sum[i] -= dT_ms ;	
    //看到这里需要讲解一下匿名代码是怎么检测静止的
    //把前面说到的角速度差值放大3倍然后累加起来，
    //每毫秒-1，如果这个值大于10说明飞机还没实现自稳
    //如果小于10说明飞机已经基本实现自稳
		g_d_sum[i] = LIMIT(g_d_sum[i],0,200);
    //把这个值限定在0-200之间
		if( g_d_sum[i] > 10)
		{
			t++;
		}
		
		g_old[i] = sensor.Gyro_Original[i];
	}

	if(t>=2)
	{
		flag.motionless = 0;	
		//如果出现3次大于0则可判定飞机未自稳
	}
	else
	{
		flag.motionless = 1;
	}

}
//我们看完这个函数发现这个函数最主要作用就是返回一个自稳系数
//flag.motionless，如果motionless=1则可认为实现了自稳，
//如果为0则可认为没有实现自稳。
//flag是匿名代码贯穿始终的一个结构体所以最好遇到的时候最好注意点，
//方便之后理解代码
typedef struct
{
	//基本状态/传感器
	u8 start_ok;//初始化All_Init()这个值就为恒定的1
	u8 sensor_imu_ok;
	u8 mems_temperature_ok;
	
	u8 motionless;//自稳系数，如果为1，实现自稳，如果为0，则没有实行自稳
	u8 power_state;
	u8 wifi_ch_en;
	u8 chn_failsafe;
	u8 rc_loss;	
	u8 rc_loss_back_home;
	u8 gps_ok;	

	
	//控制状态
	u8 manual_locked;
	u8 unlock_err;
	u8 unlock_cmd;
	u8 unlock_sta;//unlocked
	u8 thr_low;
	u8 locking;
	u8 taking_off; //起飞
	u8 set_yaw;
	u8 ct_loc_hold;
	u8 ct_alt_hold;

	
	//飞行状态
	u8 flying;
	u8 auto_take_off_land;
	u8 home_location_ok;	
	u8 speed_mode;
	u8 thr_mode;	
	u8 flight_mode;
	u8 flight_mode2;
	u8 gps_mode_en;
	u8 motor_preparation;
	u8 locked_rotor;
}_flag;

如有错误希望在评论区指出
排版不佳