stm32MPU6050得到角度的代码

以下是基于STM32使用MPU6050获取角度的示例代码，使用I2C通信协议：

#include "main.h"
#include "math.h"
#include "stdio.h"
#include "stdlib.h"
#include "string.h"

#define MPU6050_ADDR 0xD0
#define MPU6050_SMPLRT_DIV 0x19
#define MPU6050_CONFIG 0x1A
#define MPU6050_GYRO_CONFIG 0x1B
#define MPU6050_ACCEL_CONFIG 0x1C
#define MPU6050_ACCEL_XOUT_H 0x3B
#define MPU6050_ACCEL_XOUT_L 0x3C
#define MPU6050_ACCEL_YOUT_H 0x3D
#define MPU6050_ACCEL_YOUT_L 0x3E
#define MPU6050_ACCEL_ZOUT_H 0x3F
#define MPU6050_ACCEL_ZOUT_L 0x40
#define MPU6050_TEMP_OUT_H 0x41
#define MPU6050_TEMP_OUT_L 0x42
#define MPU6050_GYRO_XOUT_H 0x43
#define MPU6050_GYRO_XOUT_L 0x44
#define MPU6050_GYRO_YOUT_H 0x45
#define MPU6050_GYRO_YOUT_L 0x46
#define MPU6050_GYRO_ZOUT_H 0x47
#define MPU6050_GYRO_ZOUT_L 0x48
#define MPU6050_PWR_MGMT_1 0x6B
#define MPU6050_WHO_AM_I 0x75

I2C_HandleTypeDef hi2c1;

void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_I2C1_Init(void);

uint8_t MPU6050_WriteReg(uint8_t reg, uint8_t data);
uint8_t MPU6050_ReadReg(uint8_t reg);
void MPU6050_Init(void);
void MPU6050_GetAccelRawData(int16_t* pData);
void MPU6050_GetGyroRawData(int16_t* pData);
float MPU6050_GetAngleX(void);
float MPU6050_GetAngleY(void);
void MPU6050_GetAngle(float* pAngleX, float* pAngleY);

int main(void)
{
  HAL_Init();
  SystemClock_Config();
  MX_GPIO_Init();
  MX_I2C1_Init();

  MPU6050_Init();

  float angleX = 0;
  float angleY = 0;

  while (1)
  {
    MPU6050_GetAngle(&angleX, &angleY);
    printf("angleX: %.2f, angleY: %.2f\r\n", angleX, angleY);
    HAL_Delay(100);
  }
}

uint8_t MPU6050_WriteReg(uint8_t reg, uint8_t data)
{
  uint8_t buffer[2];
  buffer[0] = reg;
  buffer[1] = data;
  return HAL_I2C_Master_Transmit(&hi2c1, MPU6050_ADDR, buffer, 2, 1000);
}

uint8_t MPU6050_ReadReg(uint8_t reg)
{
  uint8_t data = 0;
  HAL_I2C_Master_Transmit(&hi2c1, MPU6050_ADDR, &reg, 1, 1000);
  HAL_I2C_Master_Receive(&hi2c1, MPU6050_ADDR, &data, 1, 1000);
  return data;
}

void MPU6050_Init(void)
{
  MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x00);
  MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x07);
  MPU6050_WriteReg(MPU6050_CONFIG, 0x06);
  MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);
  MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x01);
}

void MPU6050_GetAccelRawData(int16_t* pData)
{
  uint8_t buffer[6];
  HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, MPU6050_ACCEL_XOUT_H, 1, buffer, 6, 1000);
  pData[0] = (int16_t)((buffer[0] << 8) | buffer[1]);
  pData[1] = (int16_t)((buffer[2] << 8) | buffer[3]);
  pData[2] = (int16_t)((buffer[4] << 8) | buffer[5]);
}

void MPU6050_GetGyroRawData(int16_t* pData)
{
  uint8_t buffer[6];
  HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, MPU6050_GYRO_XOUT_H, 1, buffer, 6, 1000);
  pData[0] = (int16_t)((buffer[0] << 8) | buffer[1]);
  pData[1] = (int16_t)((buffer[2] << 8) | buffer[3]);
  pData[2] = (int16_t)((buffer[4] << 8) | buffer[5]);
}

float MPU6050_GetAngleX(void)
{
  int16_t accelData[3] = {0};
  MPU6050_GetAccelRawData(accelData);

  float accelX = (float)accelData[0] / 16384.0;
  float accelY = (float)accelData[1] / 16384.0;
  float accelZ = (float)accelData[2] / 16384.0;

  float angleX = atan2f(accelY, sqrtf(accelX * accelX + accelZ * accelZ));
  angleX = angleX * 180 / M_PI;
  return angleX;
}

float MPU6050_GetAngleY(void)
{
  int16_t accelData[3] = {0};
  MPU6050_GetAccelRawData(accelData);

  float accelX = (float)accelData[0] / 16384.0;
  float accelY = (float)accelData[1] / 16384.0;
  float accelZ = (float)accelData[2] / 16384.0;

  float angleY = atan2f(-accelX, sqrtf(accelY * accelY + accelZ * accelZ));
  angleY = angleY * 180 / M_PI;
  return angleY;
}

void MPU6050_GetAngle(float* pAngleX, float* pAngleY)
{
  int16_t gyroData[3] = {0};
  MPU6050_GetGyroRawData(gyroData);

  float gyroX = (float)gyroData[0] / 131.0;
  float gyroY = (float)gyroData[1] / 131.0;

  *pAngleX += gyroX * 0.1;
  *pAngleY += gyroY * 0.1;

  *pAngleX = *pAngleX * 0.98 + MPU6050_GetAngleX() * 0.02;
  *pAngleY = *pAngleY * 0.98 + MPU6050_GetAngleY() * 0.02;
}

void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
  {
    Error_Handler();
  }
}

static void MX_I2C1_Init(void)
{
  hi2c1.Instance = I2C1;
  hi2c1.Init.Timing = 0x0000020B;
  hi2c1.Init.OwnAddress1 = 0;
  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c1.Init.OwnAddress2 = 0;
  hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
  if (HAL_I2C_Init(&hi2c1) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK)
  {
    Error_Handler();
  }
}

static void MX_GPIO_Init(void)
{
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();
}

void Error_Handler(void)
{
}

#ifdef  USE_FULL_ASSERT
void assert_failed(uint8_t *file, uint32_t line)
{ 
}
#endif

在上述代码中，`MPU6050_GetAngleX()`和`MPU6050_GetAngleY()`函数分别获取X轴和Y轴的倾角，`MPU6050_GetAngle()`函数则利用陀螺仪的数据进行角度修正，得到最终的倾角。可以通过调用`MPU6050_GetAngle()`函数获取实时的倾角。