//Madgwick фильтр
//https://robotclass.ru/articles/madgwick-filter/3/
//Проверено для гироскопа и гироплатформы. Решить вопрос по курсу
#include "I2Cdev.h"
#include "MPU6050.h"
#include "MadgwickAHRS.h"
#include <Servo.h>

#define TO_RAD 0.01745329252f

MPU6050 accelgyro;
Servo servo1;
Servo servo2;

unsigned long tm, imu_t, prn_t;
const unsigned int imu_to = 20;
const unsigned int prn_to = 100;

float tdelta, X, Y;
int16_t ax, ay, az;
int16_t gx_raw, gy_raw, gz_raw;

float imu[3];
float quat[4];
float e[3];

void setup() {
  Serial.begin(9600);
  Serial.println("Initializing Servo...");
  servo1.attach(11);
  servo2.attach(10);
  servo1.write(90);
  servo2.write(90);
  delay(1000);
  Serial.println("Initializing I2C devices...");
  accelgyro.initialize(); // инициализация датчиков
  Serial.println("Testing device connections...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  delay(3000);
}

void loop() {
  accelgyro.getMotion6(&ax, &ay, &az, &gx_raw, &gy_raw, &gz_raw);

  // преобразуем сырые данные гироскопа в рад/с
  //везде где мы умножаем на TO_RAD — мы переводим градусы в радианы,
  //а там где мы делим на TO_RAD — наоборот, переводим радианы в градусы
  float gx = gx_raw * TO_RAD / 131.0;
  float gy = gy_raw * TO_RAD / 131.0;
  float gz = gz_raw * TO_RAD / 131.0;

  // каждые 20мс вычисляем углы наклона
  tm = millis();
  if (imu_t + imu_to < tm) {
    tdelta = tm - imu_t; // вычисляем дельту времени в миллисекундах
    imu_t = tm;
    // вызываем алгоритм фильтра и передаем туда:
    // - дельту времени в секундах
    // - данные гироскопа в рад/с
    // - сырые данные акселерометра
    MadgwickAHRSupdateIMU(tdelta / 1000.0, gx, gy, gz, (float)ax, (float)ay, (float)az);
    quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
    quat2Euler(&quat[0], &imu[0]); // преобразуем кватернион в углы Эйлера
  }

  // каждые 100мс добавляем новую точку графика
  tm = millis();
  if (prn_t + prn_to < tm) {
    prn_t = tm;
    Serial.print(imu[0] / TO_RAD); Serial.print("\t");
    Serial.print(imu[1] / TO_RAD); Serial.print("\t");
    Serial.println(imu[2] / TO_RAD);
  }

  servo1.write( map(imu[0] / TO_RAD, -90, 90, 0, 180) );
  servo2.write( map(imu[1] / TO_RAD, -90, 90, 0, 180) );
  delay(10);
}