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