From 5d2491912555c4549f6683ebc9c8b375553bb126 Mon Sep 17 00:00:00 2001 From: Martin Date: Fri, 4 Mar 2022 21:58:05 +0100 Subject: [PATCH 1/3] Added conversion from quat to euler for rotation difference and added it to messaging --- osc32bt/osc32bt.ino | 56 +++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 54 insertions(+), 2 deletions(-) diff --git a/osc32bt/osc32bt.ino b/osc32bt/osc32bt.ino index da1764d..8390fee 100644 --- a/osc32bt/osc32bt.ino +++ b/osc32bt/osc32bt.ino @@ -2,10 +2,15 @@ #include "Wire.h" #include "MPU6050_6Axis_MotionApps20.h" - +#include "BasicLinearAlgebra.h" #include "BluetoothSerial.h" #include #include "SLIPEncodedBluetoothSerial.h" +#include +#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 @@ -200,6 +205,22 @@ void loop() { SLIPSerial.endPacket(); quaternionDiffMessage.empty(); + + 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(); #endif @@ -263,7 +284,6 @@ void loop() { msg.add(AcZ); msg.add(elapsedTime); - SLIPSerial.beginPacket(); msg.send(SLIPSerial); SLIPSerial.endPacket(); @@ -291,3 +311,35 @@ void loop() { kmsg.empty(); } } + + +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)); +} \ No newline at end of file From 21b5674b273ddccd9c7774ffb45cefb0ceac381d Mon Sep 17 00:00:00 2001 From: Martin Date: Fri, 4 Mar 2022 23:22:05 +0100 Subject: [PATCH 2/3] added position and speed calculation and messaging --- osc32bt/osc32bt.ino | 159 ++++++++++++++++++++++---------------------- 1 file changed, 79 insertions(+), 80 deletions(-) diff --git a/osc32bt/osc32bt.ino b/osc32bt/osc32bt.ino index 8390fee..73403b5 100644 --- a/osc32bt/osc32bt.ino +++ b/osc32bt/osc32bt.ino @@ -63,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 @@ -74,6 +74,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,6 +93,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(); @@ -101,6 +106,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"); @@ -158,6 +168,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; @@ -180,47 +234,23 @@ 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 @@ -275,6 +305,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); @@ -283,63 +318,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); } -} - - -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)); } \ No newline at end of file From 63990f2242ce38e151bec200978a43393d7ab448 Mon Sep 17 00:00:00 2001 From: Martin Date: Sat, 5 Mar 2022 02:04:56 +0100 Subject: [PATCH 3/3] added timer var --- osc32final/osc32final.ino | 2 ++ 1 file changed, 2 insertions(+) diff --git a/osc32final/osc32final.ino b/osc32final/osc32final.ino index db1ade1..e975dd9 100644 --- a/osc32final/osc32final.ino +++ b/osc32final/osc32final.ino @@ -107,6 +107,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 +uint32_t timeOn = 0; // Uptime counter for movement calculation + // Sem dobimo vrednosti pospeskomerja in ziroskopa int16_t AcX,AcY,AcZ;