基于STM32F103C8T6的三轴云台MPU6050数据异常求助

问题概述 #

使用STM32F103C8T6蓝板、3个MG996R舵机和MPU6050制作三轴云台，采用I2CDev库与Mahony滤波库，目前遇到以下问题：

• 采集到的数据几乎不受MPU6050姿态变化影响
• 加速度计经误差校准后数据恢复正常，但陀螺仪静止时数值大幅波动
• 经过Mahony滤波处理后问题仍未改善

相关代码 #

#include "main.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "I2Cdev.h"
#include "MPU6050.h"
#include <stdint.h>
#include <stm32f1xx.h>
#include <STM32F103Xb.h>
//#include <MadgwickAHRS.h>
#include <MahonyAHRS.h>
#include <math.h>

I2C_HandleTypeDef hi2c1;

TIM_HandleTypeDef htim2;

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

int main(void)
{

    SystemInit();
    HAL_Init();
 
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */
  __I2C1_CLK_ENABLE();
  hi2c1.Instance = I2C1;
  hi2c1.Init.ClockSpeed = 400000;
  hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
  hi2c1.Init.OwnAddress1 = 0x10;
  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLED;
  hi2c1.Init.OwnAddress2 = 0x11;
  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLED;
  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLED;
  HAL_I2C_Init(&hi2c1);

  I2Cdev_init(&hi2c1); // init of i2cdevlib.
  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_I2C1_Init();
  MX_TIM2_Init();
  while (!MPU6050_testConnection())
  {
      MPU6050_initialize();
      HAL_Delay(1000);
      char buf[128];

      // --> enable interrupt
      MPU6050_resetSensors();   // --> Reset all sensor registers and signal paths.
      MPU6050_setIntDataReadyEnabled(1);
      MPU6050_resetAccelerometerPath();
      MPU6050_resetGyroscopePath();
      // --> CALCULATING THE ERROR FOR ACC AND GYRO

      float AccErrorX,AccErrorY,AccErrorZ =0;

      for(int i=0;i<200;i++){
          HAL_Delay(MPU6050_getAccelerometerPowerOnDelay());

              float Ax = MPU6050_getAccelerationX()/8192; // --> gravity is affecting the values
              float Ay = MPU6050_getAccelerationY()/8192;
              float Az = MPU6050_getAccelerationZ()/8192;
              AccErrorX = AccErrorX + Ax;
              AccErrorY = AccErrorY + Ay;
              AccErrorZ = AccErrorZ + Az;


      }
      AccErrorX = AccErrorX / 200;
      AccErrorY = AccErrorY / 200;
      AccErrorZ = AccErrorZ / 200;


      float GyroErrorX=0;
      float GyroErrorY=0;
      float GyroErrorZ=0;
      for(int i=0;i<200;i++){
          HAL_Delay(MPU6050_getAccelerometerPowerOnDelay());

              float Gx = MPU6050_getRotationX()/131.0;
              float Gy = MPU6050_getRotationY()/131.0;
              float Gz = MPU6050_getRotationZ()/131.0;
              GyroErrorX = GyroErrorX + (Gx / 131.0);
              GyroErrorY = GyroErrorY + (Gy / 131.0);
              GyroErrorZ = GyroErrorZ + (Gz / 131.0);

      }
      GyroErrorX = GyroErrorX / 200;
      GyroErrorY = GyroErrorY / 200;
      GyroErrorZ = GyroErrorZ / 200;


      // --> set Offset for x,y and z for accelerometer adn gyro

      MPU6050_setZAccelOffset(1551); //1551

      MPU6050_setXGyroOffset(17); //17
      MPU6050_setYGyroOffset(-69); //-69
      MPU6050_setZGyroOffset(27); //27

      HAL_Delay(100);
      MPU6050_resetSensors();
      while (1)
      {
          // --> check if data register is ready to be read

          HAL_Delay(4);

          if(MPU6050_getIntDataReadyStatus()){

              // --> Acceleration & Gyroscope values in X, Y and Z, to calculate quaternion values

              float Ax = (MPU6050_getAccelerationX()/8192); // --> gravity is affecting the values
              float Ay = (MPU6050_getAccelerationY()/8192);
              float Az = (MPU6050_getAccelerationZ()/8192)-3;

              float Gx = (MPU6050_getRotationX()/131.0)-132.198471;
              float Gy = (MPU6050_getRotationY()/131.0)+250.137405;
              float Gz = (MPU6050_getRotationZ()/131.0)-209.328247;

              // --> Get Quaternions

              MahonyAHRSupdateIMU(Gx, Gy, Gz, Ax, Ay, Az);
              float myq0 = q0;
              float myq1 = q1;
              float myq2 = q2;
              float myq3 = q3;

              // -->  Convert quaternion to Euler angles (yaw, pitch, roll)

              float yaw, pitch, roll;

              yaw = atan2(2*(myq0*myq3 + myq1*myq2), 1 - 2*(myq2*myq2 + myq3*myq3));
              pitch = asin(2*(myq0*myq2 - myq1*myq3));
              roll = atan2(2*(myq0*myq1 + myq2*myq3), 1 - 2*(myq1*myq1 + myq2*myq2));

              // --> Convert radians to degrees if necessary

              yaw *= 180.0 / M_PI;
              pitch *= 180.0 / M_PI;
              roll *= 180.0 / M_PI;

              // --> Now yaw, pitch, and roll contain the orientation angles in degrees
          }
      }
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
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_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL7;
  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_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV8;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

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

  /** Enables the Clock Security System
  */
  HAL_RCC_EnableCSS();
}

static void MX_I2C1_Init(void)
{

}

static void MX_TIM2_Init(void)
{

  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};
  TIM_OC_InitTypeDef sConfigOC = {0};
  htim2.Instance = TIM2;
  htim2.Init.Prescaler = 0;
  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim2.Init.Period = 65535;
  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
  if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
  {
    Error_Handler();
  }
  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 0;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_ENABLE;
  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
  {
    Error_Handler();
  }
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM2_Init 2 */

  /* USER CODE END TIM2_Init 2 */
  HAL_TIM_MspPostInit(&htim2);

}

static void MX_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  __HAL_RCC_GPIOD_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3, GPIO_PIN_RESET);

  /*Configure GPIO pin : PA3 */
  GPIO_InitStruct.Pin = GPIO_PIN_3;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

}

void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT

void assert_failed(uint8_t *file, uint32_t line)
{

}
#endif /* USE_FULL_ASSERT */

排查建议 #

• 修正陀螺仪校准逻辑：代码中计算陀螺仪误差时重复做了除法，Gx已转成角速度值（°/s），无需再次除以131.0，应直接累加Gx到误差变量中。
• 关联校准结果与硬件偏移：当前手动设置的偏移值和校准计算出的GyroErrorX/Y/Z无关联，需将计算得到的误差值转换为MPU6050寄存器对应的16位整数偏移量（灵敏度131LSB/(°/s)），再调用设置偏移的函数。
• 移除无效手动偏移修正：主循环中给传感器数据加的硬编码偏移值无依据，且MPU6050已通过硬件偏移设置完成校准，应直接使用原始数据或用计算出的误差值做软件修正。
• 解决I2C初始化冲突：手动初始化hi2c1后又调用空的MX_I2C1_Init()，可能导致配置异常，需统一初始化逻辑，确保I2C参数配置正确。
• 优化数据读取机制：HAL_Delay(4)+轮询数据就绪标志的方式易导致数据读取不及时，建议改用MPU6050数据就绪中断的回调函数处理数据。

内容的提问来源于stack exchange，提问作者Paula