【CW32饭盒派开发板试用体验】I2C1+ MPU6050

【CW32饭盒派开发板试用体验】+MPU6050测试

1. 硬件资源介绍

本次实验主要测试了I2C1和MPU6050。上位机串口助手通过USART3(PA9,PA10)与主板进行通讯，

https://detail.tmall.com/item.htm?_u=t21pacl09d24&id=676443688650&spm=a1z09.2.0.0.14b22e8dJieKSJ

1. 测评流程
2. 移植编写I2C1和 MPU6050驱动代码。
3. 测试MPU6050初始化和校准。
4. 获取传感器数据，解析欧拉角。
5. 代码介绍
   1. 代码结构介绍，此处移植https://oshwhub.com/yuliuqing111/xin-yuan4-zhou-fei-xing-qi驱飞控资源驱动代码，非常感谢资源。

1. I2C1初始化
   // IIC配置函数
   void IIC_Mode_Config(void)
   {
   __RCC_GPIOB_CLK_ENABLE();
   __RCC_I2C1_CLK_ENABLE();
   // 配置 IIC
   GPIO_InitTypeDef GPIO_InitStructure; // 声明 GPIO端口初始化配置
   // GPIO_InitStructure.IT = GPIO_IT_NONE; // 中断方式：选择 不使用(此时需要中断，故注释掉)
   PB06_AFx_I2C1SCL(); // PB10 SCL
   PB07_AFx_I2C1SDA(); // PB11 SDA
   GPIO_InitStructure.Pins = GPIO_PIN_6 | GPIO_PIN_7; // 选择引脚：PB10(SCL) & PB11(SDA)引脚
   GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_OD; // 模式：开漏输出
   GPIO_InitStructure.Speed = GPIO_SPEED_HIGH; // 速度：选择 高速
   GPIO_Init(CW_GPIOB, &GPIO_InitStructure); // 初始化 PB10(SCL) & PB11(SDA)引脚

   I2C_InitTypeDef I2C_InitStruct; // 声明 IIC初始化结构体

   I2C_InitStruct.I2C_BaudEn = ENABLE; // 使能 波特率时钟（决定 通信速率）

   I2C_InitStruct.I2C_Baud = 0x13; // 通信速率 400Kbps：400KHz = 64MHz÷8÷(19+1)

   I2C_InitStruct.I2C_AA = ENABLE; // ACK配置

   I2C_InitStruct.I2C_FLT = DISABLE; // FLT配置

   I2C1_DeInit();

   I2C_Master_Init(CW_I2C1, &I2C_InitStruct); // 初始化

   I2C_Cmd(CW_I2C1,ENABLE); // 使能
   }

2. MPU6050初始化
   // 初始化 MPU6050
   void MPU6050_Init(void)
   {
   // 初始化之前 需延迟一段时间，否则 断电后再上电 数据可能出错
   int i=0,j=0;
   for(i=0;i<10;i++)
   {
   for(j=0;j<100;j++)
   {
   ;
   }
   }
   // delay1ms(100);
   // MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x80); // 复位
   //
   MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x00); // 解除 休眠状态，唤醒 MPU6050

MPU6050_WriteReg(MPU6050_RA_SMPLRT_DIV , 0x02); // 陀螺仪采样率 0x02

MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x03); // 时钟源 陀螺仪Z轴

MPU6050_WriteReg(MPU6050_RA_CONFIG, 0x03); // 数字低通滤波 0x03(42Hz)

MPU6050_WriteReg(MPU6050_RA_GYRO_CONFIG, 0x18); // 陀螺仪 满量程范围（灵敏度）±2000deg/s

// 存疑：B站代码 0x09，CSDN示例 1<<3(0x08)
MPU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG, 0x09); // 加速度计 满量程范围（灵敏度）±4g

//USART_SendString(DEBUG_USARTy, "MPU6050 初始化成功！\r\n");
printf((const char *)"MPU6050 初始化成功！\r\n");
}

3. 读取传感器数据
   void MpuGetData(void)
   {
   uint8_t i;
   uint8_t buffer[12];

   Acc_Read();
   Gyro_Read();

   for(i=0;i<6;i++)
   {
   pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i];
   if(i < 3)
   {
   static struct _1_ekf_filter ekf[3] = {{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543}};
   kalman_1(&ekf[i],(float)pMpu[i]); // 一维卡尔曼滤波
   pMpu[i] = (int16_t)ekf[i].out;
   }

   if(i > 2)
   {
   uint8_t k = i - 3;

// const float factor = 0.15f; // 滤波因子
const float factor = 0.15f; // 滤波因子
static float tBuff[3];

pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;
}
}
}

5. 解析欧拉角。

void GetAngle(const _st_Mpu *pMpu,_st_AngE *pAngE, float dt)
{
volatile struct V
{
float x;
float y;
float z;
} Gravity,Acc,Gyro,AccGravity;

static struct V GyroIntegError = {0};
static float KpDef = 0.8f ;
static float KiDef = 0.0003f;
static Quaternion NumQ = {1, 0, 0, 0};
float q0_t,q1_t,q2_t,q3_t;

//float NormAcc;
float NormQuat;
float HalfTime = dt * 0.5f;
// 提取等效旋转矩阵中的重力分量
Gravity.x = 2*(NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);
Gravity.y = 2*(NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);
Gravity.z = 1-2*(NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);
// 加速度归一化
NormAcc = Q_rsqrt(squa(MPU6050.accX)+ squa(MPU6050.accY) +squa(MPU6050.accZ));

Acc.x = pMpu->accX * NormAcc;
Acc.y = pMpu->accY * NormAcc;
Acc.z = pMpu->accZ * NormAcc;
//向量差乘得出的值
AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);
AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);
AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);
//再做加速度积分补偿角速度的补偿值
GyroIntegError.x += AccGravity.x * KiDef;
GyroIntegError.y += AccGravity.y * KiDef;
GyroIntegError.z += AccGravity.z * KiDef;
//角速度融合加速度积分补偿值
Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x + GyroIntegError.x;//弧度制
Gyro.y = pMpu->gyroY * Gyro_Gr + KpDef * AccGravity.y + GyroIntegError.y;
Gyro.z = pMpu->gyroZ * Gyro_Gr + KpDef * AccGravity.z + GyroIntegError.z;
// 一阶龙格库塔法, 更新四元数

q0_t = (-NumQ.q1Gyro.x - NumQ.q2Gyro.y - NumQ.q3Gyro.z) * HalfTime;
q1_t = ( NumQ.q0Gyro.x - NumQ.q3Gyro.y + NumQ.q2Gyro.z) * HalfTime;
q2_t = ( NumQ.q3Gyro.x + NumQ.q0Gyro.y - NumQ.q1Gyro.z) * HalfTime;
q3_t = (-NumQ.q2Gyro.x + NumQ.q1Gyro.y + NumQ.q0Gyro.z) * HalfTime;

NumQ.q0 += q0_t;
NumQ.q1 += q1_t;
NumQ.q2 += q2_t;
NumQ.q3 += q3_t;
// 四元数归一化
NormQuat = Q_rsqrt(squa(NumQ.q0) + squa(NumQ.q1) + squa(NumQ.q2) + squa(NumQ.q3));
NumQ.q0 *= NormQuat;
NumQ.q1 *= NormQuat;
NumQ.q2 *= NormQuat;
NumQ.q3 *= NormQuat;

// 四元数转欧拉角
{
#ifdef YAW_GYRO

// *(float *)pAngE = atan2f(2 * NumQ.q1 *NumQ.q2 + 2 * NumQ.q0 * NumQ.q3, 1 - 2 * NumQ.q2 *NumQ.q2 - 2 * NumQ.q3 * NumQ.q3) * RtA; //yaw
pAngE->yaw = atan2f(2 * NumQ.q1 *NumQ.q2 + 2 * NumQ.q0 * NumQ.q3, 1 - 2 * NumQ.q2 *NumQ.q2 - 2 * NumQ.q3 * NumQ.q3) * RtA; //yaw
#else
float yaw_G = pMpu->gyroZ * Gyro_G;
if((yaw_G > 3.0f) || (yaw_G < -3.0f)) //数据太小可以认为是干扰，不是偏航动作
{
pAngE->yaw += yaw_G * dt;
}
#endif
pAngE->pitch = asin(2 * NumQ.q0 *NumQ.q2 - 2 * NumQ.q1 * NumQ.q3) * RtA;
pAngE->roll = atan2(2 * NumQ.q2 *NumQ.q3 + 2 * NumQ.q0 * NumQ.q1, 1 - 2 * NumQ.q1 *NumQ.q1 - 2 * NumQ.q2 * NumQ.q2) * RtA; //PITCH
}
}

四.MPU6050驱动移植结束