---

#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;


// ==========================================
// ДАННЫЕ В ИСХОДНОМ БАЗИСЕ
// ==========================================

// Ускорения, м/с^2
float ax, ay, az;

// Угловые скорости, рад/с
float wx, wy, wz;

// Температура, °C
float temperature;


// ==========================================
// МАТРИЦА ПЕРЕХОДА
//
//       | 0  -1  0 |
// T  =  | 1   0  0 |
//       | 0   0  1 |
//
// ==========================================

float T[3][3] = {
  {0, -1, 0},
  {1,  0, 0},
  {0,  0, 1}
};


// Векторы в исходном базисе
float aS[3];
float wS[3];

// Векторы в новом базисе
float aB[3];
float wB[3];


// Компоненты в новом базисе
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;


// ==========================================
// ФУНКЦИЯ 0...360 ГРАДУСОВ
// ==========================================

float angle360(float angle) {

  while (angle < 0) {
    angle += 360.0;
  }

  while (angle >= 360.0) {
    angle -= 360.0;
  }

  return angle;
}


// ==========================================
// ЧТЕНИЕ MPU6050
// ==========================================

void readMPU() {

  Wire.beginTransmission(DEV_ADDR);

  // Начинаем чтение с ACCEL_XOUT_H
  Wire.write(0x3B);

  Wire.endTransmission(false);

  // Читаем 14 байт:
  // AX AY AZ TEMP GX GY GZ
  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;


  // Температура -> °C
  temperature = temp_raw / 340.0 + 36.53;


  // Гироскоп:
  // сырые данные -> град/с -> рад/с

  wx = gx_raw / 131.0 * PI / 180.0;
  wy = gy_raw / 131.0 * PI / 180.0;
  wz = gz_raw / 131.0 * PI / 180.0;
}


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

void changeBasis() {

  // Формируем исходный вектор ускорения
  aS[0] = ax;
  aS[1] = ay;
  aS[2] = az;


  // Формируем исходный вектор
  // угловой скорости
  wS[0] = wx;
  wS[1] = wy;
  wS[2] = wz;


  // Перед перемножением
  // обнуляем результирующие векторы

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

    aB[i] = 0;
    wB[i] = 0;
  }


  // ========================================
  // aB = T * aS
  // ========================================

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

    for (int j = 0; j < 3; j++) {

      aB[i] += T[i][j] * aS[j];
    }
  }


  // ========================================
  // wB = T * wS
  // ========================================

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

    for (int j = 0; j < 3; j++) {

      wB[i] += T[i][j] * wS[j];
    }
  }


  // Получаем компоненты в новом базисе

  aBx = aB[0];
  aBy = aB[1];
  aBz = aB[2];


  p = wB[0];
  q = wB[1];
  r = wB[2];
}


// ==========================================
// SETUP
// ==========================================

void setup() {

  Serial.begin(115200);

  Wire.begin();


  // ========================================
  // БУДИМ MPU6050
  // ========================================

  Wire.beginTransmission(DEV_ADDR);

  Wire.write(0x6B);   // PWR_MGMT_1

  Wire.write(0);      // выход из режима сна

  Wire.endTransmission(true);


  // ========================================
  // ГИРОСКОП ±250 град/с
  // ========================================

  Wire.beginTransmission(DEV_ADDR);

  Wire.write(0x1B);

  Wire.write(0);

  Wire.endTransmission(true);


  // ========================================
  // АКСЕЛЕРОМЕТР ±2g
  // ========================================

  Wire.beginTransmission(DEV_ADDR);

  Wire.write(0x1C);

  Wire.write(0);

  Wire.endTransmission(true);


  delay(100);


  // ========================================
  // КАЛИБРОВКА ГИРОСКОПА ПО Z
  //
  // ДАТЧИК НЕ ДВИГАТЬ!
  // ========================================

  float sum = 0;

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

    readMPU();

    changeBasis();

    sum += r;

    delay(3);
  }


  // Средняя ошибка по Z
  beta_z = sum / 500.0;


  // ========================================
  // НАЧАЛЬНЫЕ УГЛЫ
  // ========================================

  readMPU();

  changeBasis();


  // Начальный крен по акселерометру
  roll = atan2(aBy, aBz);


  // Начальный тангаж по акселерометру
  pitch = atan2(
    -aBx,
    sqrt(aBy * aBy + aBz * aBz)
  );


  // Начальный Yaw принимаем за 0
  yaw = 0;


  lastTime = micros();
}


// ==========================================
// ОСНОВНОЙ ЦИКЛ
// ==========================================

void loop() {

  // Читаем MPU6050
  readMPU();


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

  changeBasis();


  // ========================================
  // СЧИТАЕМ dt
  // ========================================

  unsigned long now = micros();

  float dt =
    (now - lastTime) / 1000000.0;

  lastTime = now;


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

  Serial.println("===== ИСХОДНЫЕ ДАННЫЕ =====");


  Serial.print("ax = ");
  Serial.print(ax);

  Serial.print("  ay = ");
  Serial.print(ay);

  Serial.print("  az = ");
  Serial.print(az);

  Serial.println(" м/с^2");


  Serial.print("wx = ");
  Serial.print(wx, 4);

  Serial.print("  wy = ");
  Serial.print(wy, 4);

  Serial.print("  wz = ");
  Serial.print(wz, 4);

  Serial.println(" рад/с");


  Serial.print("Температура = ");
  Serial.print(temperature);
  Serial.println(" C");


  // ========================================
  // ЗАДАНИЕ 1 — ВЫВОД
  // ========================================

  Serial.println("===== ЗАДАНИЕ 1 =====");


  Serial.print("aBx = ");
  Serial.print(aBx);

  Serial.print("  aBy = ");
  Serial.print(aBy);

  Serial.print("  aBz = ");
  Serial.print(aBz);

  Serial.println(" м/с^2");


  Serial.print("p = ");
  Serial.print(p, 4);

  Serial.print("  q = ");
  Serial.print(q, 4);

  Serial.print("  r = ");
  Serial.print(r, 4);

  Serial.println(" рад/с");


  // ========================================
  // ЗАДАНИЕ 2
  // ========================================


  // ----------------------------------------
  // УГЛЫ ПО АКСЕЛЕРОМЕТРУ
  // ----------------------------------------

  // Крен
  float roll_acc =
    atan2(aBy, aBz);


  // Тангаж
  float pitch_acc =
    atan2(
      -aBx,
      sqrt(aBy * aBy + aBz * aBz)
    );


  // Сохраняем старые значения углов
  float oldRoll = roll;
  float oldPitch = pitch;


  // ========================================
  // ROLL — КРЕН
  // ========================================

  roll =
    (1.0 - alpha) *
    (
      oldRoll +

      dt *
      (
        p
        + q * sin(oldRoll) * tan(oldPitch)
        + r * cos(oldRoll) * tan(oldPitch)
      )
    )

    + alpha * roll_acc;


  // ========================================
  // PITCH — ТАНГАЖ
  // ========================================

  pitch =
    (1.0 - alpha) *
    (
      oldPitch +

      dt *
      (
        q * cos(oldRoll)
        - r * sin(oldRoll)
      )
    )

    + alpha * pitch_acc;


  // ========================================
  // YAW — РЫСКАНИЕ
  // ========================================

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


  // ========================================
  // ПЕРЕВОД В ГРАДУСЫ
  // И ПРИВЕДЕНИЕ К 0...360
  // ========================================

  float roll_deg =
    angle360(roll * 180.0 / PI);


  float pitch_deg =
    angle360(pitch * 180.0 / PI);


  float yaw_deg =
    angle360(yaw * 180.0 / PI);


  // ========================================
  // ЗАДАНИЕ 2 — ВЫВОД
  // ========================================

  Serial.println("===== ЗАДАНИЕ 2 =====");


  Serial.print("Крен (Roll) = ");
  Serial.print(roll_deg);
  Serial.println(" град");


  Serial.print("Тангаж (Pitch) = ");
  Serial.print(pitch_deg);
  Serial.println(" град");


  Serial.print("Рыскание (Yaw) = ");
  Serial.print(yaw_deg);
  Serial.println(" град");


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


  delay(100);
}