//https://x-io.co.uk/open-source-imu-and-ahrs-algorithms
//https://github.com/xioTechnologies/Fusion
//https://www.youtube.com/watch?v=7GVXqNLLH7Q
#include "Fusion.h"
#include <stdbool.h>
#include <stdio.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Servo.h>
#include <Wire.h>

#define SAMPLE_PERIOD (0.01f) // replace this with actual sample period
#define TO_RAD 0.01745329252f // этот коэффициент нужен нам для перевода градусов в радианы

FusionAhrs ahrs;
MPU6050 accelgyro;
Servo servo1;
Servo servo2;
Servo servo3;

int16_t ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw;

void setup() {
  Wire.begin();
  Serial.begin(9600);
  servo1.attach(11);
  servo2.attach(10);
  FusionAhrsInitialise(&ahrs);
  // инициализация MPU6050
  accelgyro.initialize();
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  delay(3000);
}

void loop() {
  // получаем сырые данные от датчиков в MPU6050
  accelgyro.getMotion6(&ax_raw, &ay_raw, &az_raw, &gx_raw, &gy_raw, &gz_raw);
  // преобразуем сырые данные гироскопа в рад/с
  float gx = gx_raw * TO_RAD / 131.0;
  float gy = gy_raw * TO_RAD / 131.0;
  float gz = gz_raw * TO_RAD / 131.0;

  // this loop should repeat each time new gyroscope data is available
  const FusionVector gyroscope = {gx, gy, gz}; // replace this with actual gyroscope data in degrees/s
  const FusionVector accelerometer =  {(float)ax_raw, (float)ay_raw, (float)az_raw * 1.0}; // replace this with actual accelerometer data in g

  FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD);
  const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));

  //Serial.printf("Roll %0.1f, Pitch %0.1f, Yaw %0.1f\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
  Serial.print("Крен "); //Крен Roll
  Serial.print(euler.angle.roll);
  Serial.print(" Тангаж "); //Тангаж Pitch
  Serial.print(euler.angle.pitch);
  Serial.print("  Рыскание "); //Рыскание Yaw
  Serial.println(euler.angle.yaw);

  servo1.write( map(euler.angle.roll, -10, 10, 0, 180) );
  servo2.write( map(euler.angle.pitch, -10, 10, 0, 180) );
  //servo3.write( map(euler.angle.yaw) );

  delay(5);
}