/*
  Arduino Nano 33 BLE - Sensor Fusion

  This example reads values from LSM9DS1 sensor 
  and continuously feeds them trough the Sensor Fusion library.

  The circuit:
  - Arduino Nano 33 BLE Sense

  This example code is in the public domain.
*/

#include <Arduino_LSM9DS1.h>

#include "SensorFusion.h" //SF
SF fusion;

float gx, gy, gz, ax, ay, az, mx, my, mz;
float pitch, roll, yaw;
float deltat;

//Gyro Offset....: -605622 -24028 -338386
//Gyro Offset: -6835327.00 12884521.00 918579.13

float goffx = -605622.0 / 1000000.0;
float goffy = -24028.0 / 1000000.0;
float goffz =   -338386.0 / 1000000.0;

void setup() {

  Serial.begin(115200); //serial to display data

 while (!Serial);
  Serial.println("Started");

  // IMU begin code

  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }
  Serial.print("Magnetic field sample rate = ");
  Serial.print(IMU.magneticFieldSampleRate());
  Serial.println(" uT");
  Serial.println();
  Serial.println("Magnetic Field in uT");
  Serial.println("X\tY\tZ");
  
  Serial.print("Accelerometer sample rate = ");
  Serial.print(IMU.accelerationSampleRate());
  Serial.println(" Hz");
  Serial.println();
  Serial.println("Acceleration in G's");
  Serial.println("X\tY\tZ");

  Serial.print("Gyroscope sample rate = ");
  Serial.print(IMU.gyroscopeSampleRate());
  Serial.println(" Hz");
  Serial.println();
  Serial.println("Gyroscope in degrees/second");
  Serial.println("X\tY\tZ");



}

void loop() {
 // now you should read the gyroscope, accelerometer (and magnetometer if you have it also)
    // NOTE: the gyroscope data have to be in radians
    // if you have them in degree convert them with: DEG_TO_RAD example: gx * DEG_TO_RAD

  if (IMU.magneticFieldAvailable()) {
    IMU.readMagneticField(mx, my, mz);
  }

  if (IMU.accelerationAvailable()&&IMU.gyroscopeAvailable()) {
    IMU.readAcceleration(ax, ay, az);
  /*
    ax *= 9.81;  
    ay *= 9.81;  
    az *= 9.81;  
  */
  
    IMU.readGyroscope(gx, gy, gz);

    gx -= goffx;
    gy -= goffy;
    gz -= goffz;

    gx *= DEG_TO_RAD;
    gy *= DEG_TO_RAD;
    gz *= DEG_TO_RAD;
  
   
    deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update
    //choose only one of these two:
    //fusion.MahonyUpdate(gx, gy, gz, ax, ay, az, deltat);  //mahony is suggested if there isn't the mag and the mcu is slow
    fusion.MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat);  //else use the magwick, it is slower but more accurate

    pitch = fusion.getPitch();
    roll = fusion.getRoll();    //you could also use getRollRadians() ecc
    yaw = fusion.getYaw();

    Serial.print("Orientation: "); Serial.print(-roll);
    Serial.print(" "); Serial.print(-pitch);
    Serial.print(" "); Serial.println(yaw);
  }
}