added sensor fusion calibration and diagnostic arduino sketches for arduino nano 33 ble

ArduinoNano33BLE/SensorFusionNano33BLE/SensorFusionNano33BLE.ino

@@ -0,0 +1,108 @@
+/*
+  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);
+  }
+}

ArduinoNano33BLE/SensorFusionNano33BLE_calibration/SensorFusionNano33BLE_calibration.ino

@@ -0,0 +1,117 @@
+/*
+  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));
+  }
+  }
+}