---

#include <Wire.h>

#define DEV_ADDR 0x68

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;

float beta_z = 0;

const float alpha = 0.02;

unsigned long lastTime;


void readMPU() {

  Wire.beginTransmission(DEV_ADDR);
  Wire.write(0x3B);
  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;


  // Переход в новый базис
  aBx = -ay;
  aBy =  ax;
  aBz =  az;

  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;


  // Начальные углы
  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;


  // =====================================
  // ИСХОДНЫЕ ДАННЫЕ
  // =====================================

  Serial.println("===== SOURCE MPU6050 =====");

  Serial.print("ax = ");
  Serial.print(ax);
  Serial.print("  ay = ");
  Serial.print(ay);
  Serial.print("  az = ");
  Serial.print(az);
  Serial.println(" m/s^2");

  Serial.print("wx = ");
  Serial.print(wx);
  Serial.print("  wy = ");
  Serial.print(wy);
  Serial.print("  wz = ");
  Serial.print(wz);
  Serial.println(" rad/s");


  // =====================================
  // ЗАДАНИЕ 1
  // ПЕРЕХОД В НОВЫЙ БАЗИС
  // =====================================

  Serial.println("===== TASK 1 =====");

  Serial.print("aBx = ");
  Serial.print(aBx);
  Serial.print("  aBy = ");
  Serial.print(aBy);
  Serial.print("  aBz = ");
  Serial.print(aBz);
  Serial.println(" m/s^2");

  Serial.print("p = ");
  Serial.print(p);
  Serial.print("  q = ");
  Serial.print(q);
  Serial.print("  r = ");
  Serial.print(r);
  Serial.println(" rad/s");


  // =====================================
  // ЗАДАНИЕ 2
  // ROLL, PITCH, YAW
  // =====================================

  float roll_acc = atan2(aBy, aBz);

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


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


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


  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.println("===== TASK 2 =====");

  Serial.print("Roll = ");
  Serial.print(roll_deg);
  Serial.print(" deg   ");

  Serial.print("Pitch = ");
  Serial.print(pitch_deg);
  Serial.print(" deg   ");

  Serial.print("Yaw = ");
  Serial.print(yaw_deg);
  Serial.println(" deg");

  Serial.println("==============================");

  delay(100);
}