STM32F407标准库函数对MPU6050陀螺仪航向角有零飘进行矫正代码

以下是一个基于 STM32F407 标准库函数的 MPU6050 陀螺仪航向角零飘校准代码示例，供参考：

#include "stm32f4xx.h"
#include "stm32f4xx_i2c.h"
#include "stm32f4xx_gpio.h"
#include "stm32f4xx_rcc.h"

#define MPU6050_I2C_ADDR  0x68

I2C_InitTypeDef I2C_InitStruct;
GPIO_InitTypeDef GPIO_InitStruct;

void I2C_Configuration(void) {
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);

  GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_9;
  GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
  GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStruct.GPIO_OType = GPIO_OType_OD;
  GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
  GPIO_Init(GPIOB, &GPIO_InitStruct);

  GPIO_PinAFConfig(GPIOB, GPIO_PinSource6, GPIO_AF_I2C1);
  GPIO_PinAFConfig(GPIOB, GPIO_PinSource9, GPIO_AF_I2C1);

  I2C_InitStruct.I2C_ClockSpeed = 400000;
  I2C_InitStruct.I2C_Mode = I2C_Mode_I2C;
  I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2;
  I2C_InitStruct.I2C_OwnAddress1 = 0x00;
  I2C_InitStruct.I2C_Ack = I2C_Ack_Enable;
  I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
  I2C_Init(I2C1, &I2C_InitStruct);

  I2C_Cmd(I2C1, ENABLE);
}

void MPU6050_WriteReg(uint8_t reg, uint8_t value) {
  while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY));
  I2C_GenerateSTART(I2C1, ENABLE);
  while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));
  I2C_Send7bitAddress(I2C1, MPU6050_I2C_ADDR, I2C_Direction_Transmitter);
  while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
  I2C_SendData(I2C1, reg);
  while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
  I2C_SendData(I2C1, value);
  while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
  I2C_GenerateSTOP(I2C1, ENABLE);
}

uint8_t MPU6050_ReadReg(uint8_t reg) {
  uint8_t value;
  while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY));
  I2C_GenerateSTART(I2C1, ENABLE);
  while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));
  I2C_Send7bitAddress(I2C1, MPU6050_I2C_ADDR, I2C_Direction_Transmitter);
  while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
  I2C_SendData(I2C1, reg);
  while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
  I2C_GenerateSTART(I2C1, ENABLE);
  while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));
  I2C_Send7bitAddress(I2C1, MPU6050_I2C_ADDR, I2C_Direction_Receiver);
  while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));
  while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
  value = I2C_ReceiveData(I2C1);
  I2C_GenerateSTOP(I2C1, ENABLE);
  return value;
}

void MPU6050_Init(void) {
  MPU6050_WriteReg(0x6B, 0x00); // 解除休眠状态
  MPU6050_WriteReg(0x1B, 0x08); // 陀螺仪量程设置为±500°/s
}

void MPU6050_Calibrate(int16_t *gx_offset, int16_t *gy_offset, int16_t *gz_offset) {
  int32_t gx_sum = 0, gy_sum = 0, gz_sum = 0;
  int16_t gx, gy, gz;

  for (int i = 0; i < 1000; i++) {
    gx = (int16_t)((uint16_t)MPU6050_ReadReg(0x43) << 8 | MPU6050_ReadReg(0x44));
    gy = (int16_t)((uint16_t)MPU6050_ReadReg(0x45) << 8 | MPU6050_ReadReg(0x46));
    gz = (int16_t)((uint16_t)MPU6050_ReadReg(0x47) << 8 | MPU6050_ReadReg(0x48));
    gx_sum += gx;
    gy_sum += gy;
    gz_sum += gz;
    delay(2);
  }

  *gx_offset = gx_sum / 1000;
  *gy_offset = gy_sum / 1000;
  *gz_offset = gz_sum / 1000;
}

void MPU6050_GetRotation(int16_t *gx, int16_t *gy, int16_t *gz, int16_t gx_offset, int16_t gy_offset, int16_t gz_offset) {
  *gx = (int16_t)((uint16_t)MPU6050_ReadReg(0x43) << 8 | MPU6050_ReadReg(0x44)) - gx_offset;
  *gy = (int16_t)((uint16_t)MPU6050_ReadReg(0x45) << 8 | MPU6050_ReadReg(0x46)) - gy_offset;
  *gz = (int16_t)((uint16_t)MPU6050_ReadReg(0x47) << 8 | MPU6050_ReadReg(0x48)) - gz_offset;
}

int main(void) {
  int16_t gx_offset, gy_offset, gz_offset;
  int16_t gx, gy, gz;

  I2C_Configuration();
  MPU6050_Init();
  MPU6050_Calibrate(&gx_offset, &gy_offset, &gz_offset);

  while (1) {
    MPU6050_GetRotation(&gx, &gy, &gz, gx_offset, gy_offset, gz_offset);
    printf("gx = %d, gy = %d, gz = %d\n", gx, gy, gz);
    delay(2);
  }
}

这段代码首先进行了 I2C 和 GPIO 的初始化，然后进入主函数。在主函数中，首先进行了 MPU6050 的初始化和设置，然后进行了零飘校准。在校准的过程中，通过采集 1000 次陀螺仪的读数，计算出零飘的偏移量。最后，通过 MPU6050_GetRotation 函数获取校准后的陀螺仪读数，并输出到串口。