// Библиотека для работы фильтра Madgwick
#include <TroykaIMU.h>
// Библиотека для работы IMU-сенсора LSM6DSOXTR
#include <Arduino_LSM6DSOX.h>

// Создаём объект для фильтра Madgwick
Madgwick filter;

// Переменные для данных с гироскопа и акселерометра
float gx, gy, gz, ax, ay, az;

// Переменные для хранения самолётных углов ориентации
float yaw, pitch, roll;

// Переменная для хранения частоты выборок фильтра
float sampleRate = 100;

void setup() {
    // Открываем последовательный порт
    Serial.begin(9600);
    // Выводим сообщение об неудачной инициализации IMU
    if (!IMU.begin()) {
      Serial.println("Failed to initialize IMU!");
    }
    // Выводим сообщение об удачной инициализации IMU
    Serial.println("Success to initialize IMU");
    // Инициализируем фильтр
    filter.begin();
}

void loop() {
    // Запоминаем текущее время
    unsigned long startMillis = millis();

    // Считываем данные с акселерометра в единицах G
    if (IMU.accelerationAvailable()) {
      IMU.readAcceleration(ax, ay, az);
    }
    // Считываем данные с гироскопа в градусах
    if (IMU.gyroscopeAvailable()) {
      IMU.readGyroscope(gx, gy, gz);
    }
    // Переводим показания гироскопа изградусов в радианы в секунду
    gx *= DEG_TO_RAD;
    gy *= DEG_TO_RAD;
    gz *= DEG_TO_RAD;
    // Устанавливаем частоту фильтра
    filter.setFrequency(sampleRate);
    // Обновляем входные данные в фильтр
    filter.update(gx, gy, gz, ax, ay, az);

    if (Serial.available() > 0) {
        int val = Serial.read();
        // Если пришёл символ 's'
        if (val == 's') {
            float q0, q1, q2, q3;
            filter.readQuaternion(q0, q1, q2, q3);
            // Выводим кватернион в serial-порт
            Serial.print(q0);
            Serial.print(",");
            Serial.print(q1);
            Serial.print(",");
            Serial.print(q2);
            Serial.print(",");
            Serial.println(q3);
        }
    }
    // Вычисляем затраченное время на обработку данных
    unsigned long deltaMillis = millis() - startMillis;
    // Вычисляем частоту обработки фильтра
    sampleRate = 1000 / deltaMillis;
}