From 618547af5203734f0e574d38aec64d33e5b193c2 Mon Sep 17 00:00:00 2001
From: Martin Krauser <martin.krauser@gmail.com>
Date: Fri, 4 Mar 2022 13:39:40 +0100
Subject: [PATCH] Added the quaternion difference to qmsg

---
 osc32bt/osc32bt.ino | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/osc32bt/osc32bt.ino b/osc32bt/osc32bt.ino
index f54c507..2fade66 100644
--- a/osc32bt/osc32bt.ino
+++ b/osc32bt/osc32bt.ino
@@ -160,15 +160,25 @@ void loop() {
 
 #ifdef OUTPUT_READABLE_QUATERNION
     // display quaternion values in easy matrix form: w x y z
+    Quaternion previousQ(q.w,q.x,q.y,q.z);
+
     mpu.dmpGetQuaternion(&q, fifoBuffer);
     
-    //oscmsg = qOSC(q.w, q.x, q.y, q.z);
+    //compute the differential rotation between the previous and new orientation
+    Quaternion diff = q.getProduct(previousQ.getConjugate());
+
+    //oscmsg = qOSC(q.w, q.x, q.y, q.z, diff.w, diff.x, diff.y, diff,z);
     
     qmsg.add(q.w);
     qmsg.add(q.x);
     qmsg.add(q.y);
     qmsg.add(q.z);
 
+    qmsg.add(diff.w);
+    qmsg.add(diff.x);
+    qmsg.add(diff.y);
+    qmsg.add(diff.z);
+
     SLIPBTSerial.beginPacket();
     qmsg.send(SLIPBTSerial);
     SLIPBTSerial.endPacket();