Question

我正在尝试使用3轴陀螺仪和加速度计（MPU6050）创建人工地平线。

为了防止陀螺漂移，我使用的是互补滤波器，它将gryo和加速度计值混合在一起。

我的问题是由于我用来混合值的公式的性质，当值达到0或360度时，值将以递增的方式向上或向下计数，直到达到正确的角度。

例如：我从平面开始，我的comp滤波器值为180。然后我旋转装置，使其向上计数到360度。当装置达到360度时，我继续朝同一方向旋转。因为accYangle值已从360翻转到0，我的comp过滤器值将倒计时直到达到0度。

这是一个问题，因为我的屏幕人工地平线会向前旋转360度，然后继续朝正确的方向前进。

我已经坐下来思索了几天，想出一个解决方案，解决我如何解决这个问题，寻找改变公式的方法，但数学并不是我的优势之一，而且我得出结论我需要更大的帮助脑海中。

  // Read raw values from MPU6050
  accelgyro.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ);

  // Convert accelerometer raw values to degrees
  double accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
  double accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;

  // Convert raw gyro values to degrees per second
  double gyroXrate = (double)gyroX / 131.0;
  double gyroYrate = -((double)gyroY / 131.0);

  // Calculate angles using complementary filter
  compAngleX.myDouble = (0.93 * (compAngleX.myDouble + (gyroXrate * (double)(micros() - timer) / 1000000))) + (0.07 * accXangle);
  compAngleY.myDouble = (0.93 * (compAngleY.myDouble + (gyroYrate * (double)(micros() - timer) / 1000000))) + (0.07 * accYangle);

  timer = micros();

  // Write values to serial port
  for (int i=1; i <= 4; i++){
    Serial.write(255);
  }
  Serial.write(compAngleX.myChars, sizeof(double));
  Serial.write(compAngleY.myChars, sizeof(double));

陀螺加速度计互补滤波器

0 个答案: