integrated speed and position into bundled messaging

osc32bt/osc32bt.ino

@@ -2,10 +2,15 @@
 
 #include "Wire.h"
 #include "MPU6050_6Axis_MotionApps20.h"
-
+#include "BasicLinearAlgebra.h"
 #include "BluetoothSerial.h"
 #include <SLIPEncodedSerial.h>
 #include "SLIPEncodedBluetoothSerial.h"
+#include <BasicLinearAlgebra.h>
+#include "math.h"
+
+using namespace BLA;
+
 
 #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
 #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
@@ -58,7 +63,7 @@ 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
@@ -68,6 +73,10 @@ VectorInt16 gy;         // [x, y, z]            gyro sensor measurements
 VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
 VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
 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
@@ -85,6 +94,9 @@ OSCMessage emsg("/error/");
 OSCMessage kmsg("/keys/");
 OSCMessage quaternionMessage("/quaternion/");
 OSCMessage quaternionDiffMessage("/quaternionDiff/");
+OSCMessage eulerDiffMessage("/eulerDiff/");
+OSCMessage positionMessage("/position/");
+OSCMessage speedMessage("/speed/");
 
 void setup() {
   Wire.begin();
@@ -95,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");
 
@@ -152,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;
@@ -175,30 +236,22 @@ void loop() {
     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();
+    streamAndClearMessage(quaternionDiffMessage);
 
-    SLIPSerial.beginPacket();
-    quaternionDiffMessage.send(SLIPSerial);
-    SLIPSerial.endPacket();
+    Matrix<3> eulerDiffVector = eulerFromQuaternion(diff);
 
-    quaternionDiffMessage.empty();
+    eulerDiffMessage.add(eulerDiffVector(0));
+    eulerDiffMessage.add(eulerDiffVector(1));
+    eulerDiffMessage.add(eulerDiffVector(2));
+
+    streamAndClearMessage(eulerDiffMessage);
     
 #endif
 
@@ -232,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);
@@ -241,31 +299,26 @@ void loop() {
     msg.add(AcZ);
     msg.add(elapsedTime);
     
+    streamAndClearMessage(msg);
     
-    SLIPSerial.beginPacket();
-    msg.send(SLIPSerial);
-    SLIPSerial.endPacket();
+    positionMessage.add(position(0));
+    positionMessage.add(position(1));
+    positionMessage.add(position(2));
 
-    SLIPBTSerial.beginPacket();
-    msg.send(SLIPBTSerial);
-    SLIPBTSerial.endPacket();
+    streamAndClearMessage(positionMessage);
+
+    speedMessage.add(speed(0));
+    speedMessage.add(speed(1));
+    speedMessage.add(speed(2));
+
+    streamAndClearMessage(speedMessage);
 
-    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/osc32final.ino

@@ -6,6 +6,12 @@
 #include <OSCBundle.h>
 #include <OSCBoards.h>
 
+// POSITION CALCULATION
+#include <BasicLinearAlgebra.h>
+#include "math.h"
+
+using namespace BLA;
+
 #define SERIAL_OSC
 //#define WIFI_OSC
 //#define BT_OSC
@@ -108,6 +114,9 @@ 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
 
 // Sem dobimo vrednosti pospeskomerja in ziroskopa
 int16_t AcX,AcY,AcZ;
@@ -140,6 +149,12 @@ void setup() {
     pinMode(keys[i], INPUT_PULLUP);
   }
    
+  // Position and speed tracking
+
+  timeOn = 0;
+  position.Fill(0);
+  speed.Fill(0);
+
   // Start MPU
   mpu.initialize();
 
@@ -258,6 +273,19 @@ void loop() {
     AcZ = aaWorld.z;
 #endif
 
+   // 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;
+
+    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