---

#include <Wire.h>

#define DEV_ADDR 0x68

// Сырые данные MPU6050
int16_t ax_raw, ay_raw, az_raw;
int16_t temp_raw;
int16_t gx_raw, gy_raw, gz_raw;

// Данные в базисе датчика S
float ax, ay, az;
float wx, wy, wz;

// Данные в новом базисе B
float aBx, aBy, aBz;
float p, q, r;

// Углы
float roll = 0;
float pitch = 0;
float yaw = 0;

// Смещение гироскопа по Z
float beta_z = 0;

// Вес акселерометра в комплементарном фильтре
const float alpha = 0.02;

// Время
unsigned long lastTime;


// Чтение MPU6050
void readMPU() {

  Wire.beginTransmission(DEV_ADDR);
  Wire.write(0x3B);                 // начинаем с ACCEL_XOUT_H
  Wire.endTransmission(false);

  Wire.requestFrom(DEV_ADDR, 14, true);

  ax_raw = (Wire.read() << 8) | Wire.read();
  ay_raw = (Wire.read() << 8) | Wire.read();
  az_raw = (Wire.read() << 8) | Wire.read();

  temp_raw = (Wire.read() << 8) | Wire.read();

  gx_raw = (Wire.read() << 8) | Wire.read();
  gy_raw = (Wire.read() << 8) | Wire.read();
  gz_raw = (Wire.read() << 8) | Wire.read();


  // Акселерометр: перевод в м/с^2
  ax = ax_raw / 16384.0 * 9.81;
  ay = ay_raw / 16384.0 * 9.81;
  az = az_raw / 16384.0 * 9.81;


  // Гироскоп: сначала градусы/с,
  // потом переводим в радианы/с
  wx = gx_raw / 131.0 * PI / 180.0;
  wy = gy_raw / 131.0 * PI / 180.0;
  wz = gz_raw / 131.0 * PI / 180.0;


  // Матрица перехода:
  //
  //     | 0  -1  0 |
  // T = | 1   0  0 |
  //     | 0   0  1 |
  //
  // a_B = T * a_S

  aBx = -ay;
  aBy =  ax;
  aBz =  az;

  // omega_B = T * omega_S

  p = -wy;
  q =  wx;
  r =  wz;
}


void setup() {

  Serial.begin(115200);
  Wire.begin();


  // Разбудить MPU6050
  Wire.beginTransmission(DEV_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  delay(100);


  // Калибровка гироскопа по Z.
  // Датчик в этот момент НЕ ДВИГАТЬ.
  float sum = 0;

  for (int i = 0; i < 500; i++) {

    readMPU();

    sum += r;

    delay(3);
  }

  beta_z = sum / 500.0;


  // Начальные roll и pitch берём с акселерометра

  readMPU();

  roll = atan2(aBy, aBz);

  pitch = atan2(
    -aBx,
    sqrt(aBy * aBy + aBz * aBz)
  );

  yaw = 0;

  lastTime = micros();
}


void loop() {

  readMPU();


  // Считаем время между измерениями
  unsigned long now = micros();

  float dt = (now - lastTime) / 1000000.0;

  lastTime = now;


  // Углы по акселерометру

  float roll_acc = atan2(aBy, aBz);

  float pitch_acc = atan2(
    -aBx,
    sqrt(aBy * aBy + aBz * aBz)
  );


  // -------- ROLL --------

  roll =
    (1.0 - alpha) *
    (
      roll +
      dt *
      (
        p +
        q * sin(roll) * tan(pitch) +
        r * cos(roll) * tan(pitch)
      )
    )
    +
    alpha * roll_acc;


  // -------- PITCH --------

  pitch =
    (1.0 - alpha) *
    (
      pitch +
      dt *
      (
        q * cos(roll) -
        r * sin(roll)
      )
    )
    +
    alpha * pitch_acc;


  // -------- YAW --------

  yaw = yaw + dt * (r - beta_z);


  // Перевод радианов в градусы для вывода

  float roll_deg  = roll  * 180.0 / PI;
  float pitch_deg = pitch * 180.0 / PI;
  float yaw_deg   = yaw   * 180.0 / PI;


  Serial.print("Roll: ");
  Serial.print(roll_deg);

  Serial.print("  Pitch: ");
  Serial.print(pitch_deg);

  Serial.print("  Yaw: ");
  Serial.println(yaw_deg);


  delay(10);
}