Lots of improvements, added 14bit precision, added interleaved sending for more sensor updates.

ArduinoNano33BLE/MidiBle-tilt/KVStore.ino

@@ -0,0 +1,277 @@
+/**!  @brief Class for managing storing "Adafruit" sensor calibration 
+             in internal Flash memory with KVStore
+             for Arduino Nano 33 BLE
+ * **/
+
+//https://os.mbed.com/docs/mbed-os/v6.12/apis/kvstore.html
+//KVStore. TDBStore - Default implementation of the KVStore API. It provides static wear-leveling and quick access for when you have a small number of KV pairs.
+//https://os.mbed.com/docs/mbed-os/v6.12/apis/data-architecture.html
+
+#include "KVStore.h"
+#include "kvstore_global_api.h"
+
+  /**! XYZ vector of offsets for zero-g, in m/s^2 */
+  float accel_zerog[3] = {0, 0, 0};
+
+  /**! XYZ vector of offsets for zero-rate, in rad/s */
+  float gyro_zerorate[3] = {0, 0, 0};
+
+  /**! XYZ vector of offsets for hard iron calibration (in uT) */
+  float mag_hardiron[3] = {0, 0, 0};
+
+  /**! The 3x3 matrix for soft-iron calibration (unitless) */
+  float mag_softiron[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+
+  /**! The magnetic field magnitude in uTesla */
+  float mag_field = 50;
+
+const char* const ASC_KV_Key0 = "store_flag";
+const char* const ASC_KV_Key1 = "store_offsets";
+
+void setGyroCalibration(float x, float y, float z) {
+    gyro_zerorate[0]=x;	  
+    gyro_zerorate[1]=y;	  
+    gyro_zerorate[2]=z;	  
+}
+
+float getGyroXCal() {
+  return gyro_zerorate[0];
+}
+
+float getGyroYCal() {
+  return gyro_zerorate[1];
+}
+
+float getGyroZCal() {
+  return gyro_zerorate[2];
+}
+
+bool saveCalibration(void) {
+  Serial.println("Save Cal");
+
+  kv_reset("/kv/");
+
+  uint8_t flag=42;
+  
+  kv_set(ASC_KV_Key0,&flag,sizeof(flag),0);
+  
+  float offsets[16];
+  memcpy(offsets, accel_zerog, 12);       // 3 x 4-byte floats
+  memcpy(offsets + 3, gyro_zerorate, 12); // 3 x 4-byte floats
+  memcpy(offsets + 6, mag_hardiron, 12);  // 3 x 4-byte floats
+
+  offsets[9] = mag_field;
+
+  offsets[10] = mag_softiron[0];
+  offsets[11] = mag_softiron[4];
+  offsets[12] = mag_softiron[8];
+  offsets[13] = mag_softiron[1];
+  offsets[14] = mag_softiron[2];
+  offsets[15] = mag_softiron[5];
+
+  kv_set(ASC_KV_Key1, (uint8_t*)&offsets, sizeof(offsets), 0);
+  
+  return true;
+}
+
+bool loadCalibration(void) {
+
+  uint8_t flag;
+  
+  kv_get(ASC_KV_Key0,&flag,sizeof(flag),0); 
+
+  if (flag=!42) {
+    Serial.print("FLAG NOT SET, CALIBRATION NOT FOUND");
+    return false;
+  }
+
+  float offsets[16];
+	
+  kv_get(ASC_KV_Key1,(byte*)&offsets,sizeof(offsets),0);
+  
+  accel_zerog[0] = offsets[0];
+  accel_zerog[1] = offsets[1];
+  accel_zerog[2] = offsets[2];
+
+  gyro_zerorate[0] = offsets[3];
+  gyro_zerorate[1] = offsets[4];
+  gyro_zerorate[2] = offsets[5];
+
+  mag_hardiron[0] = offsets[6];
+  mag_hardiron[1] = offsets[7];
+  mag_hardiron[2] = offsets[8];
+
+  mag_field = offsets[9];
+
+  mag_softiron[0] = offsets[10];
+  mag_softiron[1] = offsets[13];
+  mag_softiron[2] = offsets[14];
+  mag_softiron[3] = offsets[13];
+  mag_softiron[4] = offsets[11];
+  mag_softiron[5] = offsets[15];
+  mag_softiron[6] = offsets[14];
+  mag_softiron[7] = offsets[15];
+  mag_softiron[8] = offsets[12];
+
+  return true;
+}
+
+bool printCalibration(void) {
+    Serial.println(F("------------"));
+
+  Serial.print("Accelerometer: ");
+  Serial.print(accel_zerog[0]);
+  Serial.print(", ");
+  Serial.print(accel_zerog[1]);
+  Serial.print(", ");
+  Serial.print(accel_zerog[2]);
+  Serial.println();
+	
+  Serial.print("Gyroscope: ");
+  Serial.print(gyro_zerorate[0]);
+  Serial.print(", ");
+  Serial.print(gyro_zerorate[1]);
+  Serial.print(", ");
+  Serial.print(gyro_zerorate[2]);
+  Serial.println();
+	
+  Serial.print("Magnetometer Hard Iron: ");
+  Serial.print(mag_hardiron[0]);
+  Serial.print(", ");
+  Serial.print(mag_hardiron[1]);
+  Serial.print(", ");
+  Serial.print(mag_hardiron[2]);
+  Serial.println();
+	
+  Serial.print("Magnetic Field: ");
+  Serial.print(mag_field);
+  Serial.println();
+  
+  Serial.print("Magnetometer Soft Iron: ");
+  Serial.print(mag_softiron[0]);
+  Serial.print(", ");
+  Serial.print(mag_softiron[1]);
+  Serial.print(", ");
+  Serial.print(mag_softiron[2]);
+  Serial.println();
+  Serial.print(mag_softiron[3]);
+  Serial.print(", ");
+  Serial.print(mag_softiron[4]);
+  Serial.print(", ");
+  Serial.print(mag_softiron[5]);
+  Serial.println();
+  Serial.print(mag_softiron[6]);
+  Serial.print(", ");
+  Serial.print(mag_softiron[7]);
+  Serial.print(", ");
+  Serial.print(mag_softiron[8]);
+  Serial.println();
+	
+  Serial.println(F("\n------------"));
+  return true;
+}
+
+bool printSavedCalibration(void) {
+  Serial.println(F("------------"));
+
+  uint8_t flag;
+  
+  kv_get(ASC_KV_Key0,&flag,sizeof(flag),0); 
+
+  if (flag=!42) {
+    Serial.print("FLAG NOT SET, CALIBRATION NOT FOUND");
+    return false;
+  }
+
+  float offsets[16];
+	
+  kv_get(ASC_KV_Key1,(byte*)&offsets,sizeof(offsets),0);
+  
+  accel_zerog[0] = offsets[0];
+  accel_zerog[1] = offsets[1];
+  accel_zerog[2] = offsets[2];
+
+  Serial.print("Accelerometer: ");
+  Serial.print(accel_zerog[0]);
+  Serial.print(", ");
+  Serial.print(accel_zerog[1]);
+  Serial.print(", ");
+  Serial.print(accel_zerog[2]);
+  Serial.println();
+	
+  gyro_zerorate[0] = offsets[3];
+  gyro_zerorate[1] = offsets[4];
+  gyro_zerorate[2] = offsets[5];
+
+  Serial.print("Gyroscope: ");
+  Serial.print(gyro_zerorate[0]);
+  Serial.print(", ");
+  Serial.print(gyro_zerorate[1]);
+  Serial.print(", ");
+  Serial.print(gyro_zerorate[2]);
+  Serial.println();
+	
+  mag_hardiron[0] = offsets[6];
+  mag_hardiron[1] = offsets[7];
+  mag_hardiron[2] = offsets[8];
+
+  Serial.println("Magnetometer Hard Iron: ");
+  Serial.print(mag_hardiron[0]);
+  Serial.print(", ");
+  Serial.print(mag_hardiron[1]);
+  Serial.print(", ");
+  Serial.print(mag_hardiron[2]);
+  Serial.println();
+	
+  mag_field = offsets[9];
+
+  Serial.print("Magnetic Field: ");
+  Serial.print(mag_field);
+  Serial.println();
+  
+  mag_softiron[0] = offsets[10];
+  mag_softiron[1] = offsets[13];
+  mag_softiron[2] = offsets[14];
+  mag_softiron[3] = offsets[13];
+  mag_softiron[4] = offsets[11];
+  mag_softiron[5] = offsets[15];
+  mag_softiron[6] = offsets[14];
+  mag_softiron[7] = offsets[15];
+  mag_softiron[8] = offsets[12];
+
+  Serial.print("Magnetometer Soft Iron: ");
+  Serial.print(mag_softiron[0]);
+  Serial.print(", ");
+  Serial.print(mag_softiron[1]);
+  Serial.print(", ");
+  Serial.print(mag_softiron[2]);
+  Serial.println();
+  Serial.print(mag_softiron[3]);
+  Serial.print(", ");
+  Serial.print(mag_softiron[4]);
+  Serial.print(", ");
+  Serial.print(mag_softiron[5]);
+  Serial.println();
+  Serial.print(mag_softiron[6]);
+  Serial.print(", ");
+  Serial.print(mag_softiron[7]);
+  Serial.print(", ");
+  Serial.print(mag_softiron[8]);
+  Serial.println();
+	
+/*  for (uint16_t a = ee_addr; a < ee_addr + KVStore_CAL_SIZE; a++) {
+    uint8_t c = KVStore.read(a);
+    Serial.print("0x");
+    if (c < 0x10)
+      Serial.print('0');
+    Serial.print(c, HEX);
+    Serial.print(", ");
+    if ((a - ee_addr) % 16 == 15) {
+      Serial.println();
+    }
+  }
+  */
+
+  Serial.println(F("\n------------"));
+  return true;
+}

ArduinoNano33BLE/MidiBle-tilt/MidiBle-tilt.ino

@@ -1,7 +1,7 @@
 #include <Arduino_LSM9DS1.h>
-#include <BLEMIDI_Transport.h>
 #include "SensorFusion.h" //SF
 
+#include <BLEMIDI_Transport.h>
 //#include <hardware/BLEMIDI_ESP32_NimBLE.h>
 //#include <hardware/BLEMIDI_ESP32.h>
 //#include <hardware/BLEMIDI_nRF52.h>
@@ -9,6 +9,7 @@
 
 BLEMIDI_CREATE_DEFAULT_INSTANCE()
 
+unsigned long tm = millis();
 unsigned long t0 = millis();
 bool isConnected = false;
 
@@ -43,18 +44,30 @@ 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 gxoff = 0;
+float gyoff = 0;
+float gzoff = 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;
 }
 
+byte highbits(float f) {
+  return int(f) % 128;
+}
+
+byte lowbits(float f) {
+  return int(f * 128) % 128;
+};
+
+unsigned long loops = 0;
+unsigned long loopmillis = 0;
+
+unsigned long midis = 0;
+unsigned long midimillis = 0;
+
+
 // -----------------------------------------------------------------------------
 // When BLE connected, LED will turn on (indication that connection was successful)
 // When receiving a NoteOn, LED will go out, on NoteOff, light comes back on.
@@ -64,8 +77,16 @@ void setup()
 {
   Serial.begin(115200);
 
-  Serial.println("MIDI-BLE TEST 1");
-  Serial.println("2022-03-02");
+// while (!Serial) {
+    ; // wait for serial port to connect. Needed for native USB
+//  }
+
+  loopmillis = millis();
+  midimillis = millis();
+
+  Serial.println("MIDI-BLE TEST 3");
+  Serial.println("2022-06-16");
+
 
   MIDI.begin();
 
@@ -93,7 +114,21 @@ void setup()
     digitalWrite(LED_BUILTIN, HIGH);
   });
 
-  // accelerometer
+
+// gyro calibration
+ Serial.print("Loading calibration: ");
+  if (loadCalibration()) {
+    gxoff=getGyroXCal();
+    gyoff=getGyroYCal();
+    gyoff=getGyroZCal();  
+    Serial.println("success :)");
+  } else {
+      Serial.println("failed :(");
+  }
+
+  printCalibration();
+
+  // motion sensor
   if (!IMU.begin()) {
     Serial.println("Failed to initialize IMU!");
     while (1);
@@ -119,6 +154,12 @@ void setup()
   Serial.println();
   Serial.println("Magnetic Field in uT");
   Serial.println("X\tY\tZ");
+
+  Serial.print("Setup took (ms): ");
+  Serial.println(millis()-loopmillis);
+  loopmillis = millis();
+  midimillis = millis();
+  
 }
 
 // -----------------------------------------------------------------------------
@@ -126,15 +167,29 @@ void setup()
 // -----------------------------------------------------------------------------
 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()) {
+ tm = millis();
+ 
+ loops++;
+
+ if (!(loops%1000)) {
+  Serial.print("1000 loops took (ms): ");
+  Serial.println(tm-loopmillis);
+  loopmillis=tm;
+ }
+
+  MIDI.read();
+
     IMU.readAcceleration(ax, ay, az);
   /*
     ax *= 9.81;  
@@ -144,9 +199,9 @@ void loop()
   
     IMU.readGyroscope(gx, gy, gz);
 
-    gx -= goffx;
-    gy -= goffy;
-    gz -= goffz;
+    gx -= gxoff;
+    gy -= gxoff;
+    gz -= gxoff;
 
     gx *= DEG_TO_RAD;
     gy *= DEG_TO_RAD;
@@ -156,36 +211,119 @@ void loop()
     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
+    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();
 
+  if (isConnected && (tm - t0) > 6)
+  {
+    t0 = tm;
+
+    switch (midis) {
+
+
+
+      case 0:
+
+        
+        
+    maccx = fmap(ax,-4.0,4.0, 0.0,127.0);
+        MIDI.sendControlChange(0 + hand, maccx, midichannel);
+        break;
+
+      case 1:
+    maccy = fmap(ay,-4.0,4.0, 0.0,127.0);
+    MIDI.sendControlChange(1 + hand, maccy, midichannel);
+break;
+
+case 2:
+    maccz = fmap(az,-4.0,4.0, 0.0,127.0);
+    MIDI.sendControlChange(2 + hand, maccz, midichannel);
+break;
+
+case 3:
+    mgyrox = map(gx, -2000*DEG_TO_RAD, 2000*DEG_TO_RAD, 0.0, 127.0);
+    MIDI.sendControlChange(3 + hand, mgyrox, midichannel);
+break;
+
+case 4:
+    mgyroy = map(gy, -2000*DEG_TO_RAD, 2000*DEG_TO_RAD, 0.0, 127.0);
+    MIDI.sendControlChange(4 + hand, mgyroy, midichannel);
+break;
+
+case 5:
+    mgyroz = map(gz, -2000*DEG_TO_RAD, 2000*DEG_TO_RAD, 0.0, 127.0);
+    MIDI.sendControlChange(5 + hand, mgyroz, midichannel);
+break;
+
+case 6:
     accmag = sqrt(ax*ax+ay*ay+az*az);
 //    accmag = map(accmag, 0, 1000, 0,127);
     
+    MIDI.sendControlChange(6 + hand, accmag * 100, midichannel);
+break;
+
+case 7:
+    mmagx = fmap(mx, 0,60, 0,127);
+    MIDI.sendControlChange(7 + hand, mmagx, midichannel);
+break;
+
+case 8:
+    mmagy = fmap(my, 0,60, 0,127);
+    MIDI.sendControlChange(8 + hand, mmagy, midichannel);
+break;
+
+case 9:
+    mmagz = fmap(mz, 0,60, 0,127);
+    MIDI.sendControlChange(9 + hand, mmagz, midichannel);
+break;
+
+case 10:
+    roll = fusion.getRoll();    //you could also use getRollRadians() ecc
     mroll = fmap(roll,-180,180,0,127);
+    MIDI.sendControlChange(10 + hand, mroll, midichannel);
+break;
+
+case 11:
+    pitch = fusion.getPitch();
     mpitch = fmap(pitch,-90,90,0,127);
+    MIDI.sendControlChange(11 + hand, mpitch, midichannel);
+break;
+
+case 12:
+    MIDI.sendControlChange(12 + hand, myaw, midichannel);
+    yaw = fusion.getYaw();
     myaw = fmap(yaw,0,360,0,127);
+break;
+
+case 13:
+    roll = fusion.getRoll();    //you could also use getRollRadians() ecc
+    mroll = fmap(roll,-180,180,0,127);
+    MIDI.sendControlChange(13 + hand, lowbits(mroll), midichannel);
+break;
+
+case 14:
+    pitch = fusion.getPitch();
+    mpitch = fmap(pitch,-90,90,0,127);
+    MIDI.sendControlChange(14 + hand, lowbits(mpitch), midichannel);
+break;
+
+case 15:
+    MIDI.sendControlChange(15 + hand, lowbits(myaw), midichannel);
+    yaw = fusion.getYaw();
+    myaw = fmap(yaw,0,360,0,127);
+break;
+
+    }
 
-    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(gx, -2000,2000, 0.0, 127.0);
-    mgyroy = map(gy, -2000,2000, 0.0, 127.0);
-    mgyroz = map(gz, -2000,2000, 0.0, 127.0);
 //    mz = max(10, abs(z) * 1000);
-    mmagx = fmap(mx, 0,60, 0,127);
-    mmagy = fmap(my, 0,60, 0,127);
-    mmagz = fmap(mz, 0,60, 0,127);
 
-    maxx = max(magx, maxx);
-    maxy = max(magy, maxy);
-    maxz = max(magz, maxz);
+//    maxx = max(magx, maxx);
+//    maxy = max(magy, maxy);
+//    maxz = max(magz, maxz);
 
 //    Serial.print(mx);
 //    Serial.print('\t');
@@ -194,30 +332,9 @@ void loop()
 //    Serial.print('\t');
 //    Serial.println(z);
 //    Serial.println();
-  }
-  
-  MIDI.read();
 
-  
-
-  if (isConnected && (millis() - t0) > 10)
-  {
-    t0 = millis();
 
 //    MIDI.sendNoteOn (my, mx, 1); // note 60, velocity 100 on channel 1
-    MIDI.sendControlChange(0 + hand, maccx, midichannel);
-    MIDI.sendControlChange(1 + hand, maccy, midichannel);
-    MIDI.sendControlChange(2 + hand, maccz, midichannel);
-    MIDI.sendControlChange(3 + hand, mgyrox, midichannel);
-    MIDI.sendControlChange(4 + hand, mgyroy, midichannel);
-    MIDI.sendControlChange(5 + hand, mgyroz, midichannel);
-    MIDI.sendControlChange(6, accmag * 100, midichannel);
-    MIDI.sendControlChange(7 + hand, mmagx, midichannel);
-    MIDI.sendControlChange(8 + hand, mmagy, midichannel);
-    MIDI.sendControlChange(9 + hand, mmagz, midichannel);
-    MIDI.sendControlChange(10 + hand, mroll, midichannel);
-    MIDI.sendControlChange(11 + hand, mpitch, midichannel);
-    MIDI.sendControlChange(12 + hand, myaw, midichannel);
    // Serial.print(mmagx);
    // Serial.print('\t');
    // Serial.print(mmagy);
@@ -228,5 +345,25 @@ void loop()
 //    Serial.println("ping");
 //    delay(mz);
 //    MIDI.sendNoteOff(my, mx, 1);
+
+    midis=(midis+1)%16;  
+
+    if (midis==0) {
+
+    Serial.print("MIDI sending took (ms): ");
+    Serial.println(tm-midimillis);
+    midimillis=tm;
+    }
+  }
+
+
+
+
+  
   }
+  
+  
+
+  
+
 }