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);
     
-    mroll = fmap(roll,-180,180,0,127);
-    mpitch = fmap(pitch,-90,90,0,127);
-    myaw = fmap(yaw,0,360,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;
+    }
   }
+
+
+
+
+  
+  }
+  
+  
+
+  
+
 }

README.org

@@ -42,6 +42,7 @@
 ** dependencies (libraries)
 1. OSC (filter by "open sound control")
 2. MPU6050
+3. BasicLinearAlgebra
 
 ** calibration
    - IMU_Zero

osc32bt/osc32bt.ino

@@ -63,10 +63,11 @@ uint8_t devStatus;      // return status after each device operation (0 = succes
 uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
 uint16_t fifoCount;     // count of all bytes currently in FIFO
 uint8_t fifoBuffer[64]; // FIFO storage buffer
-unsigned long timeOn = 0;
+unsigned long timeOn;
 
 // orientation/motion vars
 Quaternion q;           // [w, x, y, z]         quaternion container
+Quaternion pq;          // [w, x, y, z]         quaternion container
 VectorInt16 aa;         // [x, y, z]            accel sensor measurements
 VectorInt16 gy;         // [x, y, z]            gyro sensor measurements
 VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
@@ -74,6 +75,8 @@ VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measure
 VectorFloat gravity;    // [x, y, z]            gravity vector
 float euler[3];         // [psi, theta, phi]    Euler angle container
 float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+Matrix<3> position;     // [x,y,z]              tracks position of device
+Matrix<3> speed;        // [x,y,z]              tracks speed of device
 
 
 // Sem dobimo vrednosti
@@ -91,38 +94,9 @@ OSCMessage emsg("/error/");
 OSCMessage kmsg("/keys/");
 OSCMessage quaternionMessage("/quaternion/");
 OSCMessage quaternionDiffMessage("/quaternionDiff/");
-OSCMessage eulerDiffMessage("eulerDiff");
-
-BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
-  float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
-  float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
-  float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
-  float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
-
-  BLA::Matrix<4,4> rotationMatrix = {
-    1 - (yy + zz), xy + wz, xz - wy, 0,
-    xy - wz,  1 - ( xx + zz ), yz + wx, 0,
-    xz + wy,  yz - wx, 1 - ( xx + yy ), 0,
-    0,  0,  0,  1
-  };
-
-//TODO: test whether BLA library uses column-major matrix notation in code
-  BLA::Matrix<3> eulerVector;
-  eulerVector.Fill(0);
-  eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
-  if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
-    eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
-    eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
-  } else {
-    eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
-    eulerVector(2) = 0;
-  }
-  return eulerVector;
-}
-
-float clamp(float value,float min,float max) {
-  return fmaxf( min, fminf(max, value));
-}
+OSCMessage eulerDiffMessage("/eulerDiff/");
+OSCMessage positionMessage("/position/");
+OSCMessage speedMessage("/speed/");
 
 void setup() {
   Wire.begin();
@@ -133,6 +107,11 @@ void setup() {
     pinMode(keys[i], INPUT_PULLUP);
   }
 
+  //Set position to origin, speed to nothing, and uptime to 0
+  timeOn = 0;
+  position.Fill(0);
+  speed.Fill(0);
+
   Serial.begin(115200);   // set this as high as you can reliably run on your platform
   SerialBT.begin("wavey wind");
 
@@ -190,6 +169,50 @@ void setup() {
   }
 }
 
+BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
+  float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
+  float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
+  float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
+  float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
+
+  BLA::Matrix<4,4> rotationMatrix = {
+    1 - (yy + zz), xy + wz, xz - wy, 0,
+    xy - wz,  1 - ( xx + zz ), yz + wx, 0,
+    xz + wy,  yz - wx, 1 - ( xx + yy ), 0,
+    0,  0,  0,  1
+  };
+
+//TODO: test whether BLA library uses column-major matrix notation in code
+  BLA::Matrix<3> eulerVector;
+  eulerVector.Fill(0);
+  eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
+  if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
+    eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
+    eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
+  } else {
+    eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
+    eulerVector(2) = 0;
+  }
+  return eulerVector;
+}
+
+void streamAndClearMessage(OSCMessage msg) {
+    SLIPSerial.beginPacket();
+    msg.send(SLIPSerial);
+    SLIPSerial.endPacket();
+    
+    SLIPBTSerial.beginPacket();
+    msg.send(SLIPBTSerial);
+    SLIPBTSerial.endPacket();
+    
+    msg.empty();
+}
+
+float clamp(float value,float min,float max) {
+  return fmaxf( min, fminf(max, value));
+}
+
+
 void loop() {
    // if programming failed, don't try to do anything
   if (!dmpReady) return;
@@ -212,72 +235,27 @@ void loop() {
     quaternionMessage.add(q.x);
     quaternionMessage.add(q.y);
     quaternionMessage.add(q.z);
-    
-    SLIPBTSerial.beginPacket();
-    quaternionMessage.send(SLIPBTSerial);
-    SLIPBTSerial.endPacket();
-    
-    SLIPSerial.beginPacket();
-    quaternionMessage.send(SLIPSerial);
-    SLIPSerial.endPacket();
 
-    quaternionMessage.empty();
+    streamAndClearMessage(quaternionMessage);
 
     quaternionDiffMessage.add(diff.w);
     quaternionDiffMessage.add(diff.x);
     quaternionDiffMessage.add(diff.y);
     quaternionDiffMessage.add(diff.z);
-    
-    SLIPBTSerial.beginPacket();
-    quaternionDiffMessage.send(SLIPBTSerial);
-    SLIPBTSerial.endPacket();
-    
-    SLIPSerial.beginPacket();
-    quaternionDiffMessage.send(SLIPSerial);
-    SLIPSerial.endPacket();
 
-    quaternionDiffMessage.empty();
+    streamAndClearMessage(quaternionDiffMessage);
 
     Matrix<3> eulerDiffVector = eulerFromQuaternion(diff);
 
     eulerDiffMessage.add(eulerDiffVector(0));
     eulerDiffMessage.add(eulerDiffVector(1));
     eulerDiffMessage.add(eulerDiffVector(2));
-    
-    SLIPBTSerial.beginPacket();
-    eulerDiffMessage.send(SLIPBTSerial);
-    SLIPBTSerial.endPacket();
-    
-    SLIPSerial.beginPacket();
-    eulerDiffMessage.send(SLIPSerial);
-    SLIPSerial.endPacket();
 
-    eulerDiffMessage.empty();
+    streamAndClearMessage(eulerDiffMessage);
     
 #endif
 
 
-#ifdef OUTPUT_READABLE_EULER
-    // display Euler angles in degrees
-    mpu.dmpGetQuaternion(&q, fifoBuffer);
-    mpu.dmpGetEuler(euler, &q);
-
-    GyX = euler[0];
-    GyY = euler[1];
-    GyZ = euler[2];
-#endif
-
-#ifdef OUTPUT_READABLE_YAWPITCHROLL
-    // display Euler angles in degrees
-    mpu.dmpGetQuaternion(&q, fifoBuffer);
-    mpu.dmpGetGravity(&gravity, &q);
-    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-
-    GyX = ypr[0];
-    GyY = ypr[1];
-    GyZ = ypr[2];
-#endif
-
 #ifdef OUTPUT_READABLE_REALACCEL
     // display real acceleration, adjusted to remove gravity
     mpu.dmpGetQuaternion(&q, fifoBuffer);
@@ -307,6 +285,11 @@ void loop() {
     int prevTime = timeOn;
     timeOn = millis();
     int elapsedTime = timeOn - prevTime;
+    Matrix<3> speedGain = {AcX * elapsedTime, AcY * elapsedTime, AcZ * elapsedTime};
+    
+    //Assume linear acceleration over measured time window, multiply time by halfpoint between last-known and current speed
+    position = position + (((speed + speedGain) + speed) /2 * elapsedTime);
+    speed += speedGain;
     
     mpu.dmpGetGravity(&gravity, &q);
     mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
@@ -315,33 +298,27 @@ void loop() {
     msg.add(AcY);
     msg.add(AcZ);
     msg.add(elapsedTime);
+    
+    streamAndClearMessage(msg);
+    
+    positionMessage.add(position(0));
+    positionMessage.add(position(1));
+    positionMessage.add(position(2));
+
+    streamAndClearMessage(positionMessage);
+
+    speedMessage.add(speed(0));
+    speedMessage.add(speed(1));
+    speedMessage.add(speed(2));
+
+    streamAndClearMessage(speedMessage);
+
 
-    SLIPSerial.beginPacket();
-    msg.send(SLIPSerial);
-    SLIPSerial.endPacket();
-    
-    SLIPBTSerial.beginPacket();
-    msg.send(SLIPBTSerial);
-    SLIPBTSerial.endPacket();
-    
-    msg.empty();
-    
     // Send keys
     for(int i = 0; i < KEYLEN; i++) {
       pressed[i] = !digitalRead(keys[i]);
       kmsg.add(pressed[i]);
     }
-    
-    SLIPSerial.beginPacket();
-    kmsg.send(SLIPSerial);
-    SLIPSerial.endPacket();
-    
-    SLIPBTSerial.beginPacket();
-    kmsg.send(SLIPBTSerial);
-    SLIPBTSerial.endPacket();
-    
-    kmsg.empty();
+    streamAndClearMessage(kmsg);
   }
-}
-
-
+}

osc32final/SLIPEncodedBluetoothSerial.cpp

@@ -0,0 +1,181 @@
+#include "SLIPEncodedBluetoothSerial.h"
+#include "BluetoothSerial.h"
+
+/*
+ CONSTRUCTOR
+ */
+//instantiate with the tranmission layer
+//use BluetoothSerial
+SLIPEncodedBluetoothSerial::SLIPEncodedBluetoothSerial(BluetoothSerial &s){
+	serial = &s;
+	rstate = CHAR;
+}
+
+static const uint8_t eot = 0300;
+static const uint8_t slipesc = 0333;
+static const uint8_t slipescend = 0334;
+static const uint8_t slipescesc = 0335;
+/*
+ SERIAL METHODS
+ */
+bool SLIPEncodedBluetoothSerial::endofPacket()
+{
+	if(rstate == SECONDEOT)
+	{
+		rstate = CHAR; 
+		return true;
+	}
+	if (rstate==FIRSTEOT)
+	{
+        if(serial->available())
+        {
+            uint8_t c =serial->peek();
+            if(c==eot)
+            {
+                serial->read(); // throw it on the floor
+            }
+        }
+		rstate = CHAR;
+		return true;
+	}
+	return false;
+}
+int SLIPEncodedBluetoothSerial::available(){
+back:
+	int cnt = serial->available();
+	
+	if(cnt==0)
+		return 0;
+	if(rstate==CHAR)
+	{
+		uint8_t c =serial->peek();
+		if(c==slipesc)
+		{
+			rstate = SLIPESC;
+			serial->read(); // throw it on the floor
+			goto back;
+		}
+		else if( c==eot)
+		{
+			rstate = FIRSTEOT;
+			serial->read(); // throw it on the floor
+			goto back;
+		}
+		return 1; // we may have more but this is the only sure bet
+	}
+	else if(rstate==SLIPESC)	
+		return 1;
+	else if(rstate==FIRSTEOT)
+	{
+		if(serial->peek()==eot)
+		{
+			rstate = SECONDEOT;
+			serial->read(); // throw it on the floor
+			return 0;
+		}		
+		rstate = CHAR;
+	}else if (rstate==SECONDEOT) {
+		rstate = CHAR;
+	}
+	
+	return 0;
+		
+}
+
+//reads a byte from the buffer
+int SLIPEncodedBluetoothSerial::read(){
+back:
+	uint8_t c = serial->read();
+	if(rstate==CHAR)
+	{
+		if(c==slipesc)
+		{
+			rstate=SLIPESC;
+			goto back;
+		}	
+		else if(c==eot){
+		
+			return -1; // xxx this is an error
+		}
+		return c;
+	}
+	else
+	if(rstate==SLIPESC)
+	{
+		rstate=CHAR;
+		if(c==slipescend)
+			return eot;
+		else if(c==slipescesc)
+			return slipesc;
+			else {
+				// insert some error code here
+				return -1;
+			}
+
+	}
+	else
+		return -1;
+}
+
+// as close as we can get to correct behavior
+int SLIPEncodedBluetoothSerial::peek(){
+	uint8_t c = serial->peek();
+	if(rstate==SLIPESC)
+	{
+		if(c==slipescend)
+			return eot;
+		else if(c==slipescesc)
+			return slipesc;
+	}
+	return c; 
+}
+
+//the arduino and wiring libraries have different return types for the write function
+#if defined(WIRING) || defined(BOARD_DEFS_H)
+
+//encode SLIP
+ void SLIPEncodedBluetoothSerial::write(uint8_t b){
+	if(b == eot){ 
+		serial->write(slipesc);
+		return serial->write(slipescend); 
+	} else if(b==slipesc) {  
+		serial->write(slipesc);
+		return serial->write(slipescesc); 
+	} else {
+		return serial->write(b);
+	}	
+}
+void SLIPEncodedBluetoothSerial::write(const uint8_t *buffer, size_t size) {  while(size--) write(*buffer++); }
+#else
+//encode SLIP
+size_t SLIPEncodedBluetoothSerial::write(uint8_t b){
+	if(b == eot){ 
+		serial->write(slipesc);
+		return serial->write(slipescend); 
+	} else if(b==slipesc) {  
+		serial->write(slipesc);
+		return serial->write(slipescesc); 
+	} else {
+		return serial->write(b);
+	}	
+}
+size_t SLIPEncodedBluetoothSerial::write(const uint8_t *buffer, size_t size) { size_t result=0; while(size--) result = write(*buffer++); return result; }
+
+#endif
+
+void SLIPEncodedBluetoothSerial::begin(String name){
+	serial->begin(name);
+}
+//SLIP specific method which begins a transmitted packet
+void SLIPEncodedBluetoothSerial::beginPacket() { 	serial->write(eot); }
+
+//signify the end of the packet with an EOT
+void SLIPEncodedBluetoothSerial::endPacket(){
+	serial->write(eot);
+
+}
+
+void SLIPEncodedBluetoothSerial::flush(){
+	serial->flush();
+}
+

osc32final/SLIPEncodedBluetoothSerial.h

@@ -0,0 +1,62 @@
+/*
+Extends the Serial class to encode SLIP over serial
+*/
+
+#ifndef SLIPEncodedBluetoothSerial_h
+#define SLIPEncodedBluetoothSerial_h
+
+
+#include "Arduino.h"
+#include <Stream.h>
+#include "BluetoothSerial.h"
+
+
+class SLIPEncodedBluetoothSerial: public Stream{
+	
+private:
+	enum erstate {CHAR, FIRSTEOT, SECONDEOT, SLIPESC } rstate;
+	
+	//the serial port used
+	BluetoothSerial * serial;
+
+	
+public:
+	
+	//the serial port used
+	SLIPEncodedBluetoothSerial(BluetoothSerial & );
+
+	
+	int available();
+	int read();
+	int peek();
+	void flush();
+	
+	//same as Serial.begin
+	void begin(String);
+    
+    //SLIP specific method which begins a transmitted packet
+	void beginPacket();
+	
+	//SLIP specific method which ends a transmittedpacket
+	void endPacket();
+	// SLIP specific method which indicates that an EOT was received 
+	bool endofPacket();
+	
+	
+//the arduino and wiring libraries have different return types for the write function
+#if defined(WIRING) || defined(BOARD_DEFS_H)
+	void write(uint8_t b);
+    void write(const uint8_t *buffer, size_t size);
+
+#else
+	//overrides the Stream's write function to encode SLIP
+	size_t write(uint8_t b);
+    size_t write(const uint8_t *buffer, size_t size);
+
+	//using Print::write;
+#endif
+
+};
+
+
+#endif

osc32final/osc32final.ino

@@ -3,55 +3,98 @@
 #include "Wire.h"
 #include "MPU6050_6Axis_MotionApps20.h"
 
-
+#include <OSCBundle.h>
 #include <OSCBoards.h>
-#include <OSCMessage.h>
-/*
-Make an OSC message and send it over serial
- */
 
+// POSITION CALCULATION
+#include <BasicLinearAlgebra.h>
+#include "math.h"
+
+using namespace BLA;
+
+#define SERIAL_OSC
+//#define WIFI_OSC
+#define BT_OSC
+
+#define OUTPUT_READABLE_WORLDACCEL
+
+// SERIAL
 #ifdef BOARD_HAS_USB_SERIAL
 #include <SLIPEncodedUSBSerial.h>
 SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB );
 #else
 #include <SLIPEncodedSerial.h>
- SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial
+SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial
 #endif
 
+// WIFI
+#ifdef WIFI_OSC
+#include <WiFi.h>
+  const char* ssid     = "Grajski";     // your network SSID (name of wifi network)
+  const char* password = "nedeladanes"; // your network password
+  
+  // Multicast IP / port
+  const IPAddress castIp = IPAddress(224,0,1,9);
+  const int port = 6696;
+  bool connected = false;
+
+#include <WiFiUdp.h>
+  WiFiUDP udp;
+
+  void connectToWiFi(const char * ssid, const char * pwd){
+    Serial.println("Connecting to WiFi network: " + String(ssid));
+  
+    // delete old config
+    WiFi.disconnect(true);
+    //register event handler
+    WiFi.onEvent(WiFiEvent);
+    
+    //Initiate connection
+    WiFi.begin(ssid, pwd);
+  
+    Serial.println("Waiting for WIFI connection...");
+  }
+  
+  //wifi event handler
+  void WiFiEvent(WiFiEvent_t event){
+    switch(event) {
+      case ARDUINO_EVENT_WIFI_STA_GOT_IP:
+        //When connected set 
+        Serial.print("WiFi connected! IP address: ");
+        Serial.println(WiFi.localIP());  
+        //initializes the UDP state
+        //This initializes the transfer buffer
+        udp.begin(WiFi.localIP(), port);
+        connected = true;
+        break;
+      case ARDUINO_EVENT_WIFI_STA_DISCONNECTED:
+        connected = false;
+        Serial.println("\n\n\n================\nLOST WIFI CONNECTION!\n\n\nTrying again soon...\n\n\n");
+        delay(1000);
+        connectToWiFi(ssid, password);
+        break;
+      default: break;
+    }
+  }
+#endif
+
+// Bluetooth
+#ifdef BT_OSC
+#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
+#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
+#endif
+  
+#include <SLIPEncodedSerial.h>
+#include "BluetoothSerial.h"
+#include "SLIPEncodedBluetoothSerial.h"
+  
+BluetoothSerial SerialBT;
+SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT);
+#endif
+
+// Motion sensor object
 MPU6050 mpu;
 
-// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
-// quaternion components in a [w, x, y, z] format (not best for parsing
-// on a remote host such as Processing or something though)
-#define OUTPUT_READABLE_QUATERNION
-
-// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
-// (in degrees) calculated from the quaternions coming from the FIFO.
-// Note that Euler angles suffer from gimbal lock (for more info, see
-// http://en.wikipedia.org/wiki/Gimbal_lock)
-//#define OUTPUT_READABLE_EULER
-
-// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
-// pitch/roll angles (in degrees) calculated from the quaternions coming
-// from the FIFO. Note this also requires gravity vector calculations.
-// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
-// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
-#define OUTPUT_READABLE_YAWPITCHROLL
-
-// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
-// components with gravity removed. This acceleration reference frame is
-// not compensated for orientation, so +X is always +X according to the
-// sensor, just without the effects of gravity. If you want acceleration
-// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
-//#define OUTPUT_READABLE_REALACCEL
-
-// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
-// components with gravity removed and adjusted for the world frame of
-// reference (yaw is relative to initial orientation, since no magnetometer
-// is present in this case). Could be quite handy in some cases.
-#define OUTPUT_READABLE_WORLDACCEL
-
-
 // MPU control/status vars
 bool dmpReady = false;  // set true if DMP init was successful
 uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
@@ -62,6 +105,9 @@ uint8_t fifoBuffer[64]; // FIFO storage buffer
 
 // orientation/motion vars
 Quaternion q;           // [w, x, y, z]         quaternion container
+Quaternion pq;          // [w, x, y, z]         previous quaternion container
+Quaternion diff;        // [w, x, y, z]         quaternion derivate container
+Quaternion cq;          // [w, x, y, z]         calibration quaternion
 VectorInt16 aa;         // [x, y, z]            accel sensor measurements
 VectorInt16 gy;         // [x, y, z]            gyro sensor measurements
 VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
@@ -69,9 +115,15 @@ VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measure
 VectorFloat gravity;    // [x, y, z]            gravity vector
 float euler[3];         // [psi, theta, phi]    Euler angle container
 float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+uint32_t timeOn = 0;    //                      Uptime counter for movement calculation
+Matrix<3> position;     // [x,y,z]              tracks position of device
+Matrix<3> speed;        // [x,y,z]              tracks speed of device
+Matrix<3> eulerVector;
+Matrix<3> eulerDiffVector;
+bool reset; // For quaternion calibration
 
 
-// Sem dobimo vrednosti
+// Sem dobimo vrednosti pospeskomerja in ziroskopa
 int16_t AcX,AcY,AcZ;
 float GyX, GyY, GyZ;
 
@@ -80,23 +132,69 @@ byte keys[] = {16, 17, 5, 18};
 byte pressed[] = {0, 0, 0, 0};
 byte KEYLEN = 4;
 
-OSCMessage msg("/accel/");
-OSCMessage gmsg("/gyro/");
-OSCMessage emsg("/error/");
-OSCMessage kmsg("/keys/");
-OSCMessage qmsg("/quaternion/");
+BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
+  float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
+  float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
+  float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
+  float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
+
+  BLA::Matrix<4,4> rotationMatrix = {
+    1 - (yy + zz), xy + wz, xz - wy, 0,
+    xy - wz,  1 - ( xx + zz ), yz + wx, 0,
+    xz + wy,  yz - wx, 1 - ( xx + yy ), 0,
+    0,  0,  0,  1
+  };
+  
+  //TODO: test whether BLA library uses column-major matrix notation in code
+  BLA::Matrix<3> eulerVector;
+  eulerVector.Fill(0);
+  eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
+  if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
+    eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
+    eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
+  } else {
+    eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
+    eulerVector(2) = 0;
+  }
+  return eulerVector;
+}
+
+float clamp(float value,float min,float max) {
+  return fmaxf( min, fminf(max, value));
+}
+
+
+/* OSC MSG channels */
+OSCBundle bundle;
 
 void setup() {
+  // Basic(debug) serial init
+  //  Serial.begin(115200);   // set this as high as you can reliably run on your platform
+  Serial.println("Starting up...");
+  
+  // I2C init
   Wire.begin();
   Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
-  SLIPSerial.begin(115200);   // set this as high as you can reliably run on your platform 
 
+#ifdef SERIAL_OSC
+  SLIPSerial.begin(115200);   // set this as high as you can reliably run on your platform 
+#endif
+  
   // Keys
   for(int i = 0; i < KEYLEN; i++) {
     pinMode(keys[i], INPUT_PULLUP);
   }
+   
+  // Position and speed tracking
 
+  timeOn = 0;
+  position.Fill(0);
+  speed.Fill(0);
+
+  // Start MPU
   mpu.initialize();
+
+  // Set sensitivity / range
   mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
   mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
 
@@ -104,24 +202,24 @@ void setup() {
   devStatus = mpu.dmpInitialize();
 
   // supply your own gyro offsets here, scaled for min sensitivity
-  // !!! Run Zero IMU to get readings
+  // !!! Run Zero IMU to get readings (read comments for instructions)
 
-  /* First proto (right hand, black&blue)
+  /* First proto (right hand, black&blue)*/
   mpu.setXGyroOffset(76);
   mpu.setYGyroOffset(68);
   mpu.setZGyroOffset(10);
   mpu.setXAccelOffset(-3527);
   mpu.setYAccelOffset(-913);
   mpu.setZAccelOffset(1027);
-  */
 
-  /* Second proto, translucent / white */
+  /* Second proto, translucent / white 
   mpu.setXGyroOffset(-3650);
   mpu.setYGyroOffset(-2531);
   mpu.setZGyroOffset(1131);
   mpu.setXAccelOffset(162);
   mpu.setYAccelOffset(-16);
   mpu.setZAccelOffset(-12);
+  */
   
   // make sure it worked (returns 0 if so)
   if (devStatus == 0) {
@@ -141,65 +239,65 @@ void setup() {
     // get expected DMP packet size for later comparison
     packetSize = mpu.dmpGetFIFOPacketSize();
   } else {
-    Serial.println("Error: " + String(devStatus));
-    emsg.add("DMP Initialization failed (code " + String(devStatus) + ")");
-    SLIPSerial.beginPacket();
-    emsg.send(SLIPSerial);
-    SLIPSerial.endPacket();
-    emsg.empty();
+    Serial.println("DMP Initialization failed (code " + String(devStatus) + ")");
     // ERROR!
     // 1 = initial memory load failed
     // 2 = DMP configuration updates failed
     // (if it's going to break, usually the code will be 1)
   }
+
+#ifdef WIFI_OSC
+  // WIFI init
+  Serial.print("Attempting to connect to SSID: ");
+  Serial.println(ssid);
+  connectToWiFi(ssid, password);
+
+  // attempt to connect to Wifi network:
+  while (WiFi.status() != WL_CONNECTED) {
+    Serial.print(".");
+    // wait 1 second for re-trying
+    delay(1000);
+  }
+#endif
+
+#ifdef BT_OSC
+  SerialBT.begin("wavey wind");
+#endif
 }
 
 void loop() {
    // if programming failed, don't try to do anything
   if (!dmpReady) return;
+  
   // read a packet from FIFO
   if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet 
-
-
-#ifdef OUTPUT_READABLE_QUATERNION
-    // display quaternion values in easy matrix form: w x y z
-    mpu.dmpGetQuaternion(&q, fifoBuffer);
     
-    qmsg.add(q.w);
-    qmsg.add(q.x);
-    qmsg.add(q.y);
-    qmsg.add(q.z);
-    SLIPSerial.beginPacket();
-    qmsg.send(SLIPSerial);
-    SLIPSerial.endPacket();
-    qmsg.empty();
-#endif
-
-
-#ifdef OUTPUT_READABLE_EULER
-    // display Euler angles in degrees
+    // Store last Q value
+    pq = Quaternion(q.w,q.x,q.y,q.z);
+    
+    // get quaternion values in easy matrix form: w x y z
     mpu.dmpGetQuaternion(&q, fifoBuffer);
-    mpu.dmpGetEuler(euler, &q);
+    q = q.getProduct(cq);
+    
+    //compute the differential rotation between the previous and new orientation
+    diff = q.getProduct(pq.getConjugate());
+    
+    // Quaternion - rotacija
+    bundle.add("/quaternion").add(q.w).add(q.y * -1).add(q.z).add(q.x * -1); // W X Y Z
 
-    GyX = euler[0];
-    GyY = euler[1];
-    GyZ = euler[2];
-#endif
-
-#ifdef OUTPUT_READABLE_YAWPITCHROLL
-    // display Euler angles in degrees
-    mpu.dmpGetQuaternion(&q, fifoBuffer);
-    mpu.dmpGetGravity(&gravity, &q);
-    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-
-    GyX = ypr[0];
-    GyY = ypr[1];
-    GyZ = ypr[2];
-#endif
+    // Euler - rotacija
+    //eulerVector = eulerFromQuaternion(q);
+    //bundle.add("/euler").add(eulerVector(0)).add(eulerVector(1)).add(eulerVector(2)); // X Y Z
+    
+    // Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading)
+    bundle.add("/quaternionDiff").add(diff.w).add(diff.y * -1).add(diff.z).add(diff.x * -1); // W X Y Z
 
+    // Rotation diff value in euler angle
+    //eulerDiffVector = eulerFromQuaternion(diff);
+    //bundle.add("/eulerDiff").add(eulerDiffVector(0)).add(eulerDiffVector(1)).add(eulerDiffVector(2)); // X Y Z
+    
 #ifdef OUTPUT_READABLE_REALACCEL
     // display real acceleration, adjusted to remove gravity
-    mpu.dmpGetQuaternion(&q, fifoBuffer);
     mpu.dmpGetAccel(&aa, fifoBuffer);
     mpu.dmpGetGravity(&gravity, &q);
     mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
@@ -211,7 +309,6 @@ void loop() {
 #ifdef OUTPUT_READABLE_WORLDACCEL
     // display initial world-frame acceleration, adjusted to remove gravity
     // and rotated based on known orientation from quaternion
-    mpu.dmpGetQuaternion(&q, fifoBuffer);
     mpu.dmpGetAccel(&aa, fifoBuffer);
     mpu.dmpGetGravity(&gravity, &q);
     mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
@@ -219,36 +316,67 @@ void loop() {
     AcX = aaWorld.x;
     AcY = aaWorld.y;
     AcZ = aaWorld.z;
-
 #endif
 
-    // Send over serial
-    msg.add(AcX);
-    msg.add(AcY);
-    msg.add(AcZ);
-    SLIPSerial.beginPacket();
-    msg.send(SLIPSerial);
-    SLIPSerial.endPacket();
-    msg.empty();
+   // Calculate speed and position from accelerometer data
+   /*
+    int prevTime = timeOn;
+    timeOn = millis();
+    int elapsedTime = timeOn - prevTime;
+    Matrix<3> speedGain = {AcX * elapsedTime, AcY * elapsedTime, AcZ * elapsedTime};
+    
+    //Assume linear acceleration over measured time window, multiply time by halfpoint between last-known and current speed
+    position = position + (((speed + speedGain) + speed) /2 * elapsedTime);
+    speed += speedGain;
 
-    
-    gmsg.add(GyX);
-    gmsg.add(GyY);
-    gmsg.add(GyZ);
-    SLIPSerial.beginPacket();
-    gmsg.send(SLIPSerial);
-    SLIPSerial.endPacket();
-    gmsg.empty();
-    
+    bundle.add("/position/").add(position(0)).add(position(1)).add(position(2));
+    bundle.add("/speed/").add(speed(0)).add(speed(1)).add(speed(2));
+*/
+    // Accelerometer
+    bundle.add("/accel").add(AcX).add(AcY).add(AcZ); ; // X Y Z
+
+    // Keys held down
+    bundle.add("/keys"); // A B C D E
     
     // Send keys
     for(int i = 0; i < KEYLEN; i++) {
       pressed[i] = !digitalRead(keys[i]);
-      kmsg.add(pressed[i]);
+      bundle.getOSCMessage("/keys")->add(pressed[i]);
     }
+
+    // Reset calibration euler?
+    if (pressed[0] && pressed[1] && pressed[2] && pressed[3]) {
+      if (!reset) {
+        cq = q.getConjugate();
+        reset = true;
+        Serial.println("Quaternion calibrate");
+      }
+    } else {
+      if (reset) {
+        reset = false;
+      }
+    }
+    
+#ifdef SERIAL_OSC
     SLIPSerial.beginPacket();
-    kmsg.send(SLIPSerial);
+    bundle.send(SLIPSerial);
     SLIPSerial.endPacket();
-    kmsg.empty();
+#endif
+    
+  
+#ifdef WIFI_OSC
+    udp.beginPacket(castIp, port);
+    bundle.send(udp);
+    udp.endPacket();
+#endif
+
+// Some bug below, it seems
+#ifdef BT_OSC
+    SLIPBTSerial.beginPacket();
+    bundle.send(SLIPBTSerial);
+    SLIPBTSerial.endPacket();
+#endif
+
+    bundle.empty();
   }
 }

osc32wifi/osc32wifi.ino

@@ -28,8 +28,6 @@ const IPAddress castIp = IPAddress(224,0,1,9);
 const int port = 6696;
 bool connected = false;
 
-//#include "AsyncUDP.h"
-//AsyncUDP udp;
 
 #include <WiFiUdp.h>
 WiFiUDP udp;