Arduino.ru

Добрый день. 

Пытаюсь получить pitch и roll с mpu6050, но как не пытался калибровать библиотекой I2Cdev, начальные углы постоянно смещены -+ 60 градусов, через некоторое время углы приходят близко к нулю, затем переходят через 0 и дрифтуют дальше, затем опять возвращаются к нулю и так бесконечно. Также вращение платы по одной оси смещает углы по двум другим. Количество итераций калибровки и точность установки палаты по уровню незначительно влияют на результат.

Подскажите, пожалуйста, что я делаю не правильно?

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
volatile bool mpuFlag = false;  
uint8_t fifoBuffer[45]; 
        
void setup() {
  
  Serial.begin(115200);
  Wire.begin(21, 22);
  Wire.setClock(1000000UL);  
 
  mpu.initialize();
  mpu.dmpInitialize();
  mpu.setDMPEnabled(true);
  delay(1000);

  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_16);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);

  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
  
  mpu.CalibrateAccel(15);
  mpu.CalibrateGyro(15);
  
  attachInterrupt(15, dmpReady, RISING);
  mpuFlag = true;
 
}

void dmpReady() {
  mpuFlag = true;
}

void loop() {
 
  if (mpuFlag && mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
   
    Quaternion q;
    VectorFloat gravity;
    float ypr[3];
    
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    mpuFlag = false;
  
    Serial.print(degrees(ypr[0])); 
    Serial.print(',');
    Serial.print(degrees(ypr[1])); 
    Serial.print(',');
    Serial.print(degrees(ypr[2])); 
    Serial.println();
    
  }
  delay(50);
}

Монитор порта. Плата в статике и в горизонте +- 1 градус

-12.74,58.91,18.96
-12.74,58.92,18.92
-12.73,58.94,18.85
-12.75,58.95,18.82
-12.75,58.96,18.76
-12.76,58.98,18.72
-12.76,58.99,18.66
-12.76,59.00,18.62
-12.76,59.00,18.57
-12.76,59.02,18.52
-12.76,59.03,18.48
-12.76,59.04,18.44
-12.77,59.05,18.39
-12.76,59.06,18.34
-12.76,59.08,18.29
-12.77,59.08,18.25
-12.77,59.10,18.21
-12.78,59.11,18.16
-12.77,59.12,18.12
-12.78,59.13,18.08
-12.78,59.14,18.05
-12.79,59.15,18.01
-12.78,59.16,17.97
-12.79,59.17,17.92
-12.78,59.18,17.88
-12.78,59.19,17.85
-12.79,59.20,17.81
-12.79,59.21,17.77
-12.79,59.22,17.72
-12.79,59.23,17.69
-12.79,59.24,17.65
-12.80,59.25,17.62
-12.79,59.25,17.58
-12.80,59.26,17.55
-12.80,59.27,17.53
-12.80,59.28,17.49
-12.80,59.29,17.46
-12.79,59.30,17.42
-12.80,59.31,17.39
-12.81,59.31,17.35
-12.80,59.32,17.31
-12.80,59.33,17.29
-12.80,59.34,17.25
-12.80,59.35,17.22
-12.81,59.36,17.18
-12.81,59.37,17.14
-12.81,59.38,17.12
-12.81,59.38,17.09
-12.81,59.39,17.06
-12.81,59.40,17.03
-12.81,59.40,17.00
-12.81,59.41,16.97
-12.81,59.42,16.95
-12.81,59.43,16.92
-12.81,59.43,16.89
-12.82,59.45,16.86
-12.82,59.46,16.83
-12.82,59.46,16.80
-12.82,59.47,16.78
-12.83,59.48,16.75
-12.83,59.49,16.72
-12.83,59.49,16.69
-12.83,59.50,16.66
-12.84,59.50,16.64
-12.83,59.51,16.62
-12.84,59.52,16.59
-12.84,59.53,16.56
-12.84,59.53,16.53
-12.84,59.54,16.51
-12.84,59.54,16.48
-12.83,59.55,16.46
-12.84,59.56,16.44
-12.84,59.56,16.42
-12.84,59.56,16.39
-12.84,59.57,16.38
-12.84,59.57,16.35
-12.84,59.58,16.33
-12.83,59.58,16.31
-12.84,59.59,16.29
-12.84,59.60,16.26
-12.83,59.61,16.24
-12.84,59.61,16.22
-12.84,59.62,16.19
-12.83,59.62,16.18
-12.83,59.62,16.16
-12.84,59.63,16.13
-12.83,59.64,16.12
-12.84,59.64,16.10
-12.83,59.65,16.08
-12.84,59.65,16.07
-12.84,59.66,16.04
-12.83,59.66,16.03
-12.83,59.67,16.00
-12.84,59.67,15.98
-12.84,59.68,15.97
-12.83,59.68,15.94
-12.83,59.69,15.93
-12.84,59.69,15.91
-12.84,59.69,15.89
-12.84,59.70,15.88
-12.83,59.70,15.87
-12.83,59.71,15.84
-12.84,59.72,15.83
-12.84,59.72,15.82
-12.83,59.72,15.80
-12.84,59.73,15.79
-12.83,59.73,15.78
-12.83,59.74,15.74
-12.83,59.75,15.74
-12.83,59.75,15.72
-12.82,59.75,15.70
-12.82,59.76,15.68
-12.82,59.76,15.67
-12.82,59.76,15.66
-12.83,59.78,15.64
-12.82,59.78,15.63
-12.82,59.78,15.62
-12.82,59.79,15.59
-12.81,59.79,15.58
-12.81,59.79,15.57
-12.82,59.80,15.55
-12.81,59.81,15.54
-12.82,59.81,15.52
-12.81,59.81,15.51
-12.81,59.82,15.50
-12.81,59.82,15.48
-12.81,59.82,15.47
-12.80,59.82,15.46
-12.81,59.83,15.44
-12.80,59.83,15.43
-12.80,59.84,15.42
-12.80,59.84,15.42
-12.79,59.84,15.41
-12.80,59.85,15.39
-12.79,59.85,15.38
-12.79,59.85,15.37
-12.80,59.85,15.36
-12.79,59.86,15.35
-12.79,59.87,15.34
-12.80,59.87,15.32
-12.79,59.87,15.31
-12.79,59.88,15.30
-12.78,59.88,15.29
-12.78,59.88,15.29
-12.79,59.89,15.26
-12.79,59.89,15.26
-12.78,59.89,15.25
-12.78,59.90,15.24
-12.77,59.90,15.23
-12.78,59.90,15.22
-12.78,59.91,15.21
-12.78,59.91,15.20
-12.78,59.91,15.20
-12.77,59.92,15.19
-12.76,59.92,15.18
-12.76,59.92,15.18
-12.76,59.92,15.17
-12.75,59.93,15.16
-12.75,59.93,15.15
-12.75,59.94,15.15
-12.76,59.94,15.13
-12.75,59.94,15.12
-12.75,59.95,15.12
-12.75,59.95,15.11
-12.74,59.96,15.10
-12.74,59.96,15.09
-12.74,59.96,15.09
-12.74,59.97,15.08
-12.74,59.97,15.06
-12.74,59.97,15.06
-12.74,59.97,15.05
-12.74,59.97,15.05
-12.73,59.98,15.04
-12.72,59.98,15.02
-12.72,59.98,15.01
-12.72,59.98,15.01
-12.72,59.99,15.00
-12.71,59.99,14.99
-12.71,59.99,14.99
-12.71,60.00,14.98
-12.70,60.00,14.97
-12.70,60.00,14.97
-12.69,60.00,14.96
-12.69,60.00,14.95
-12.69,60.00,14.95
-12.68,60.00,14.94
-12.68,60.01,14.92
-12.67,60.01,14.91
-12.68,60.02,14.92