added sensor fusion to midible tilt

ArduinoNano33BLE/MidiBle-tilt/MidiBle-tilt.ino

@@ -1,5 +1,6 @@
 #include <Arduino_LSM9DS1.h>
 #include <BLEMIDI_Transport.h>
+#include "SensorFusion.h" //SF
 
 //#include <hardware/BLEMIDI_ESP32_NimBLE.h>
 //#include <hardware/BLEMIDI_ESP32.h>
@@ -30,9 +31,25 @@ float mgyrox, mgyroy, mgyroz;
 // magnetometer
 float magx, magy, magz;
 float mmagx, mmagy, mmagz;
+// sensor fusion 
+float mroll, mpitch, myaw;
 
 float maxx, maxy, maxz;
 
+//SF - 
+SF fusion;
+
+float gx, gy, gz, ax, ay, az, mx, my, mz;
+float pitch, roll, yaw;
+float deltat;
+
+//Gyro Offset....: -605622 -24028 -338386
+
+float goffx = -605622.0 / 1000000.0;
+float goffy = -24028.0 / 1000000.0;
+float goffz =   -338386.0 / 1000000.0;
+
+
 float fmap(float x, float in_min, float in_max, float out_min, float out_max)
 {
   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
@@ -109,27 +126,62 @@ void setup()
 // -----------------------------------------------------------------------------
 void loop()
 {
-  if (IMU.accelerationAvailable()) {
+ // now you should read the gyroscope, accelerometer (and magnetometer if you have it also)
-    IMU.readAcceleration(accx, accy, accz);
+    // NOTE: the gyroscope data have to be in radians
-    IMU.readGyroscope(gyrox, gyroy, gyroz);
+    // if you have them in degree convert them with: DEG_TO_RAD example: gx * DEG_TO_RAD
-    IMU.readMagneticField(magx, magy, magz);
 
-    accmag = sqrt(accx*accx+accy*accy+accz*accz);
+  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();
+
+    accmag = sqrt(ax*ax+ay*ay+az*az);
 //    accmag = map(accmag, 0, 1000, 0,127);
     
-    maccx = fmap(accx,-4.0,4.0, 0.0,127.0);
+    mroll = fmap(roll,-180,180,0,127);
-    maccy = fmap(accy,-4.0,4.0, 0.0,127.0);
+    mpitch = fmap(pitch,-90,90,0,127);
-    maccz = fmap(accz,-4.0,4.0, 0.0,127.0);
+    myaw = fmap(yaw,0,360,0,127);
+
+    maccx = fmap(ax,-4.0,4.0, 0.0,127.0);
+    maccy = fmap(ay,-4.0,4.0, 0.0,127.0);
+    maccz = fmap(az,-4.0,4.0, 0.0,127.0);
     //mgyrox = gyrox/2000 * 64 + 64;
     //mgyroy = gyroy/2000 * 64 + 64;
     //mgyroz = gyroz/2000 * 64 + 64;
-    mgyrox = map(gyrox, -2000,2000, 0.0, 127.0);
+    mgyrox = map(gx, -2000,2000, 0.0, 127.0);
-    mgyroy = map(gyroy, -2000,2000, 0.0, 127.0);
+    mgyroy = map(gy, -2000,2000, 0.0, 127.0);
-    mgyroz = map(gyroz, -2000,2000, 0.0, 127.0);
+    mgyroz = map(gz, -2000,2000, 0.0, 127.0);
 //    mz = max(10, abs(z) * 1000);
-    mmagx = fmap(magx, 0,60, 0,127);
+    mmagx = fmap(mx, 0,60, 0,127);
-    mmagy = fmap(magy, 0,60, 0,127);
+    mmagy = fmap(my, 0,60, 0,127);
-    mmagz = fmap(magz, 0,60, 0,127);
+    mmagz = fmap(mz, 0,60, 0,127);
 
     maxx = max(magx, maxx);
     maxy = max(magy, maxy);
@@ -163,11 +215,14 @@ void loop()
     MIDI.sendControlChange(7 + hand, mmagx, midichannel);
     MIDI.sendControlChange(8 + hand, mmagy, midichannel);
     MIDI.sendControlChange(9 + hand, mmagz, midichannel);
-    Serial.print(mmagx);
+    MIDI.sendControlChange(10 + hand, mroll, midichannel);
-    Serial.print('\t');
+    MIDI.sendControlChange(11 + hand, mpitch, midichannel);
-    Serial.print(mmagy);
+    MIDI.sendControlChange(12 + hand, myaw, midichannel);
-    Serial.print('\t');
+   // Serial.print(mmagx);
-    Serial.println(mmagz);
+   // Serial.print('\t');
+   // Serial.print(mmagy);
+   // Serial.print('\t');
+   // Serial.println(mmagz);
 //    Serial.print('\t');
 //    Serial.println(accmag * 100);
 //    Serial.println("ping");