/*
  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;

double gxavg;
double gyavg;
double gzavg;

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

float goffx = -605622.0 / 1000000.0;
float goffy = -24028 / 1000000.0;
float goffz =   -338386 / 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");

  gxavg=0;
  gyavg=0;
  gzavg=0;

}

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

    gxavg=gx*0.0001 + gxavg*0.9999;
    gyavg=gy*0.0001 + gyavg*0.9999;
    gzavg=gz*0.0001 + gzavg*0.9999;

  /*  gx -= goffx;
    gy -= goffy;
    gz -= goffz;
  */
/*    gx *= DEG_TO_RAD;
    gy *= DEG_TO_RAD;
    gz *= DEG_TO_RAD;
  */

  if (random(1000)<10) {  
    Serial.print("Gyro Offset....: "); 
    Serial.print(  int(gxavg*1000000.0)); Serial.print(" \t"); 
    Serial.print(  int(gyavg*1000000.0)); Serial.print(" \t"); 
    Serial.println(int(gzavg*1000000.0));

    Serial.print("Gyro Calibrated: "); 
    Serial.print(  int((gx-goffx)*1000000.0)); Serial.print(" \t"); 
    Serial.print(  int((gy-goffy)*1000000.0)); Serial.print(" \t"); 
    Serial.println(int((gz-goffz)*1000000.0));
  }
  }
}