Author：家有仙妻谢掌柜
Date：2021/2/18

今年会更新一个系列，小四轴无人机从功能设计→思维导图→原理图设计→PCBLayout→焊接PCB→程序代码的编写→整机调试一系列，以此记录自己的成长历程！
这个小四轴无人机是大学时期学习制作的，加上现在工作学习对嵌入式的理解更加深入，因此想要重新梳理一下小四轴，之后在此基础上实现大四轴的飞控设计，这些都将在工作之余完成！

//小四轴无人机设计

#include "mpu6500.h"

/*******************************************************************************
 * fuction	mpu6500_init
 * brief	mpu6500配置初始化
 * param	无
 * return	无
 *******************************************************************************/ 
void mpu6500_init(void)
{
	/* 复位内部寄存器，重置默认设置 */    
	// i2c_reg_write(uint8_t Device_address,uint8_t reg_address,uint8_t data);  // 对从机写操作
		i2c_reg_write(MPU6500_Addr,PWR_MGMT_1,0x80); 
		delay_ms(100);
		/* 复位陀螺仪 加速度计 温度计的配置 */    
		i2c_reg_write(MPU6500_Addr,SIGNAL_PATH_RESET,0x07);  
		/* 复位后对内部时钟的配置 */ 
		i2c_reg_write(MPU6500_Addr,PWR_MGMT_1,0x03);     
		/* 复位后对采样速率进行的配置 配置成1KHz */
		i2c_reg_write(MPU6500_Addr,SMPLRT_DIV,0x00); //  
		/* 对陀螺仪的配置 +/-2000d/s*/
		i2c_reg_write(MPU6500_Addr,GYRO_CONFIG,0x18);
		/* 对加速度的配置 +/-4g*/
		i2c_reg_write(MPU6500_Addr,ACCEL_CONFIG,0x08);
}
/*******************************************************************************
 * fuction	mpu6500_read
 * brief	mpu6500寄存器的读
 * param	无
 * return	无
 *******************************************************************************/ 
void mpu6500_read(void)
{
	//uint8_t data_buff[14]
	//i2c_reg_write(MPU6500_Addr,PWR_MGMT_1,0x80);
	int16_XYZ Old_GRY_ReadData;
	Old_GRY_ReadData.X = GRY_ReadData.X;
	Old_GRY_ReadData.Y = GRY_ReadData.Y;
	Old_GRY_ReadData.Z = GRY_ReadData.Z;

	i2c_read(MPU6500_Addr,ACCEL_XOUT_H,14,data_buff);
	//temp_throttle = (uint16_t)((( uint16_t)RF_DATA[1]<<8)|RF_DATA[2])-171.0f;
	/* 读取加速度的X Y Z的三轴数据 */
	ACC_ReadData.X = ((( int16_t)data_buff[0]<<8)|data_buff[1]);
	ACC_ReadData.Y = ((( int16_t)data_buff[2]<<8)|data_buff[3]);
	ACC_ReadData.Z = ((( int16_t)data_buff[4]<<8)|data_buff[5]);
	/* 读取陀螺仪的X Y Z的三轴数据 */
	GRY_ReadData.X = ((( int16_t)data_buff[8]<<8)|data_buff[9]);
	GRY_ReadData.Y = ((( int16_t)data_buff[10]<<8)|data_buff[11]);
	GRY_ReadData.Z = ((( int16_t)data_buff[12]<<8)|data_buff[13]);
	/*
	Old_GRY_ReadData.X = GRY_ReadData.X;
	Old_GRY_ReadData.Y = GRY_ReadData.Y;
	Old_GRY_ReadData.Z = GRY_ReadData.Z;
	*/
	/* 读取陀螺仪的X Y Z的三轴数据减去静止的偏移量 */
	if(Offset == true)
	{
		GRY_ReadData.X -= GRY_OffsetData.X;
		GRY_ReadData.Y -= GRY_OffsetData.Y;
		GRY_ReadData.Z -= GRY_OffsetData.Z;

		ACC_ReadData.X -= ACC_OffsetData.X;
		ACC_ReadData.Y -= ACC_OffsetData.Y;
		ACC_ReadData.Z -= ACC_OffsetData.Z;	
	}
	/* 判断MPU6500是否为静止 */

	Two_GRY_Len = abs(Old_GRY_ReadData.X - GRY_ReadData.X)+abs(Old_GRY_ReadData.Y - GRY_ReadData.Y) + abs(Old_GRY_ReadData.Z - GRY_ReadData.Z);
	if(Two_GRY_Len<20) 
		GRYStable_cnt++;
	else 
	{
		GRYStable_cnt = 0;
		GRYStable = false;
	}
	if(GRYStable_cnt>100) 
		GRYStable = true;
	if(GRYStable)
	{
		if(Initoffset ==  false) 
			Initoffset = true;
	}
}
/*******************************************************************************
 * fuction	get_offset
 * brief	获取加速度计和陀螺仪的偏移值
 * param	无
 * return	无
 *******************************************************************************/ 
void get_offset(void)//  读取100次 求平均
{                              
	uint8_t  i=0;
	int32_t GRY_XTemp = 0, GRY_YTemp = 0, GRY_ZTemp = 0;
	int32_t ACC_XTemp = 0, ACC_YTemp = 0, ACC_ZTemp = 0;
	/* 复位偏移标志位 */
	Offset = false;
	/* 读取100次陀螺仪和加速度的值 */
	for(i=0;i<100;i++)
	{
		mpu6500_read();
		GRY_XTemp += GRY_ReadData.X;
		GRY_YTemp += GRY_ReadData.Y;
		GRY_ZTemp += GRY_ReadData.Z;
			
		//GRY_XTemp += GRY_ReadData.X;
		ACC_XTemp += ACC_ReadData.X;
		ACC_YTemp += ACC_ReadData.Y;
		ACC_ZTemp += ACC_ReadData.Z;
			
		delay_ms(5);		
	}
	/* 读取100次陀螺仪和加速度的值并且求平均 */
	GRY_XTemp /= 100;
	GRY_YTemp /= 100;
	GRY_ZTemp /= 100;
			
	ACC_XTemp /= 100;
	ACC_YTemp /= 100;
	ACC_ZTemp /= 100;
	/* 赋值 偏移值 */
		
	GRY_OffsetData.X = GRY_XTemp;
	GRY_OffsetData.Y = GRY_YTemp;
	GRY_OffsetData.Z = GRY_ZTemp;
		
	ACC_OffsetData.X = ACC_XTemp;
	ACC_OffsetData.Y = ACC_YTemp;
	ACC_OffsetData.Z = ACC_ZTemp-8192;
		
	/* 置位偏移标志位 */
	Offset = true;		
}
/*******************************************************************************
 * fuction	Cal_Mpu_Data
 * brief	MPU值的滤波及转化
 * param	无
 * return	无
 *******************************************************************************/ 
void Cal_Mpu_Data(void)
{
	/* 转化陀螺仪为1deg/s */
	// Temp_Gry_F;     Temp_ACC_F;  Gry_Gain  ACC_Gain
	Temp_Gry_F.X = GRY_ReadData.X *Gry_Gain;
	Temp_Gry_F.Y = GRY_ReadData.Y *Gry_Gain;
	Temp_Gry_F.Z = GRY_ReadData.Z *Gry_Gain;
	//float_XYZ Gry_F;              // 无需滤波
  //float_XYZ Gry_FeedBack_F;
	
	/* 角速度进行未滤波 */
	Gry_F.X = Temp_Gry_F.X;
	Gry_F.Y = Temp_Gry_F.Y;
	Gry_F.Z = Temp_Gry_F.Z;
	/* 角速度进行低通滤波 */
	Gry_FeedBack_F.X = biquadFilterApply(&gryo_51Hz_parameter1,Temp_Gry_F.X);
	Gry_FeedBack_F.Y = biquadFilterApply(&gryo_51Hz_parameter2,Temp_Gry_F.Y);
	Gry_FeedBack_F.Z = biquadFilterApply(&gryo_51Hz_parameter3,Temp_Gry_F.Z);
	
	/* 转化加速度值为1g的值 */
	Temp_ACC_F.X = ACC_ReadData.X *ACC_Gain;
	Temp_ACC_F.Y = ACC_ReadData.Y *ACC_Gain;
	Temp_ACC_F.Z = ACC_ReadData.Z *ACC_Gain;
	
	/* 加速度进行低通滤波 */
	//&acc_30Hz_parameter1
	ACC_F.X = biquadFilterApply(&acc_30Hz_parameter1,Temp_ACC_F.X);
	ACC_F.Y = biquadFilterApply(&acc_30Hz_parameter2,Temp_ACC_F.Y);
	ACC_F.Z = biquadFilterApply(&acc_30Hz_parameter3,Temp_ACC_F.Z);
	
}
/*******************************************************************************
 * fuction	get_accmod
 * brief	获取加速度计的模长
 * param	无
 * return	AccMod
 *******************************************************************************/ 
/* 求模 x*x+y*y+z*z    然后在开/根号*/
float get_accmod(void)
{
	float AccMod = sqrtf(Temp_ACC_F.X*Temp_ACC_F.X+Temp_ACC_F.Y*Temp_ACC_F.Y+Temp_ACC_F.Z*Temp_ACC_F.Z);
	
	return AccMod;	
}

#ifndef _MPU6500_H__
#define _MPU6500_H__

#include "board_define.h"
#include "var_global.h"

//void all_init(void);
#define SMPLRT_DIV 			0x19           	// 采样速率
#define GYRO_CONFIG   		0x1B        	// 陀螺仪的配置
#define ACCEL_CONFIG  		0x1C        	// 加速度计的配置
#define ACCEL_XOUT_H 		0x3B         	// 加速度计X轴高字节（第一个字节数据）
#define SIGNAL_PATH_RESET 	0x68    		// 陀螺仪 加速度计 温度计的配置
#define PWR_MGMT_1 			0x6B           	// 上电复位配置
#define MPU6500_Addr 		0xD0         	// MPU6500从机地址
#define Gry_Gain    		0.06103f     	//
#define ACC_Gain    		0.001196f

void mpu6500_init(void);
void mpu6500_read(void);
void get_offset(void);
void Cal_Mpu_Data(void);
float get_accmod(void);

#endif