From b019145c97428a3d4e3bf7fc2125eb65a32da926 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= <jurij@kompot.si>
Date: Sun, 27 Nov 2022 09:47:28 +0100
Subject: [PATCH] Dodan quaternion, serijski link

---
 osc32_9255_rtimulib/osc32_9255_rtimulib.ino | 38 +++++++++++++++------
 1 file changed, 27 insertions(+), 11 deletions(-)

diff --git a/osc32_9255_rtimulib/osc32_9255_rtimulib.ino b/osc32_9255_rtimulib/osc32_9255_rtimulib.ino
index edbec31..c08dbc2 100644
--- a/osc32_9255_rtimulib/osc32_9255_rtimulib.ino
+++ b/osc32_9255_rtimulib/osc32_9255_rtimulib.ino
@@ -35,6 +35,16 @@ BluetoothSerial SerialBT;
 SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT);
 #endif
 
+
+// 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
+#endif
+
 // Motion sensor objects
 RTIMU *imu;                                           // the IMU object
 RTFusionRTQF fusion;                                  // the fusion object
@@ -53,7 +63,8 @@ OSCBundle bundle;
 void setup() {
   int errcode;
   // Basic(debug) serial init
-  Serial.begin(115200);   // set this as high as you can reliably run on your platform
+  SLIPSerial.begin(115200);
+  //Serial.begin(115200);   // set this as high as you can reliably run on your platform
   Serial.println("Starting up...");
   
   // Init EEPROM based on magnet calibration size requirement
@@ -130,11 +141,13 @@ void loop() {
   
     sampleCount++;
     if ((delta = now - lastRate) >= 1000) {
-      Serial.print("Sample rate: "); Serial.print(sampleCount);
-      if (!imu->IMUGyroBiasValid())
-        Serial.println(", calculating gyro bias");
-      else
-        Serial.println();
+    
+      //Serial.print("Sample rate: "); Serial.print(sampleCount);
+      if (!imu->IMUGyroBiasValid()) {
+      //  Serial.println(", calculating gyro bias");
+      } else {
+      //  Serial.println();
+      }
       
       sampleCount = 0;
       lastRate = now;
@@ -142,9 +155,6 @@ void loop() {
   
     //RTMath::display("Accel:", realAccel);
     //Serial.println();
-       
-    // Quaternion - rotacija
-    //bundle.add("/quaternion").add(fusion.getFusionQPose().scalar()).add(fusion.getFusionQPose().x()).add(fusion.getFusionQPose().y()).add(fusion.getFusionQPose().z()); // W X Y Z
   
     // Euler - rotacija
     //eulerVector = eulerFromQuaternion(q);
@@ -161,8 +171,14 @@ void loop() {
       lastDisplay = now;
       // Accelerometer
       bundle.add("/accel").add(realAccel.x()).add(realAccel.y()).add(realAccel.z());
-    
-      // Some bug below, it seems
+      
+      // Quaternion - rotacija
+      bundle.add("/quaternion").add(fusion.getFusionQPose().scalar()).add(fusion.getFusionQPose().x()).add(fusion.getFusionQPose().y()).add(fusion.getFusionQPose().z()); // W X Y Z
+
+      SLIPSerial.beginPacket();
+      bundle.send(SLIPSerial);
+      SLIPSerial.endPacket();
+  
       #ifdef BT_OSC
         SLIPBTSerial.beginPacket();
         bundle.send(SLIPBTSerial);