Получение угла крена от GY-521

Вычисляю углы тангажа и крена через комплементарный фильтр. Мне нужен только крен, и он правильно считается при небольших значениях тангажа. Но чем больше тангаж, тем неправельнее угол крена. А при тангаже около 90 градусов, крен абсолютно неверный. Как это исправить? Использовать магнитометр?

Спойлер: Код

Код (C++):

#include <Wire.h>


uint8_t IMUAddress = 0x68;

/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;

double accXangle; // Angle calculate using the accelerometer
double accYangle;
double temp;
double gyroXangle = 0; // Angle calculate using the gyro
double gyroYangle = 0;
double compAngleX = 0; // Calculate the angle using a Kalman filter
double compAngleY = 0;

uint32_t timer;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  i2cWrite(0x6B,0x00); // Disable sleep mode
  if(i2cRead(0x75,1)[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("MPU-6050 with address 0x"));
    Serial.print(IMUAddress,HEX);
    Serial.println(F(" is not connected"));
    while(1);
  }    
  timer = micros();
}

void loop() {
  /* Update all the values */
  uint8_t* data = i2cRead(0x3B,14);
  accX = ((data[0] << 8) | data[1]);
  accY = ((data[2] << 8) | data[3]);
  accZ = ((data[4] << 8) | data[5]);
  gyroX = ((data[8] << 8) | data[9]);
  gyroY = ((data[10] << 8) | data[11]);
  gyroZ = ((data[12] << 8) | data[13]);
 
  /* Calculate the angls based on the different sensors and algorithm */
  accYangle = (atan2(accX,accZ))*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ))*RAD_TO_DEG;  
 
  double gyroXrate = (double)gyroX/131.0;
  double gyroYrate = -((double)gyroY/131.0);
  gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter
  gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
  //gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
 
  compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
  compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
 
  timer = micros();

 
  /* Print Data */  
  Serial.print(accXangle);Serial.print("\t");
  Serial.print(accYangle);Serial.print("\t");
   
  Serial.print(gyroXangle);Serial.print("\t");
  Serial.print(gyroYangle);Serial.print("\t");
 
  Serial.print(compAngleX);Serial.print("\t");
  Serial.print(compAngleY); Serial.print("\t");
 
  //Serial.print(temp);Serial.print("\t");
 
  Serial.print("\n");
 
  delay(1); // The accelerometer's maximum samples rate is 1kHz
}
void i2cWrite(uint8_t registerAddress, uint8_t data){
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data);
  Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
  uint8_t data[nbytes];
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.endTransmission(false); // Don't release the bus
  Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
  for(uint8_t i = 0; i < nbytes; i++)
    data[i] = Wire.read();
  return data;
}

 

Может вам не крен нужен, а наклон относительно Земли?

 

А это не одно и тоже?

 

Нет. Крен измеряется относительно осей гироскопа. Если вам надо относительно Земли, то нужно корректировать с учетом остальных измерений.

 

Понял разницу. Но я имел ввиду именно крен.