Kalibracija, vecji baud rate sprejemnika, parent/child, druge

posodobitve - vec gui-ja, euler pretvorbe, ...

AHRSensor/AHRSensor.sc

@@ -1,9 +1,15 @@
 AHRSensor {
   var <id,
-  <>calibrationQuat,
+  <parent,
+
+  // Kalibracijski quaternion (kao nula)
+  <>calibQuat,
+
   <>quat,
   <>euler,
+  <>eulerD,
   <>accel,
+  <>accelSum,
 
   <>battery,
   <>eps,
@@ -14,30 +20,75 @@ AHRSensor {
   <gQx,
   <gQy,
   <gQz,
+  // Kalibriraj quaternion
+  <gQc,
   // Eulerjevi koti
   <gEx,
   <gEy,
   <gEz,
+  // Relativni eulerjevi koti
+  <gEdx,
+  <gEdy,
+  <gEdz,
   // Pospeskomer
   <gAx,
   <gAy,
   <gAz,
+  <gAs,
   // Baterija
   <gB,
+  <gBi,
   // Dogodkov na sekundo
   <gEps;
 
-  *new { |id|
+  *new { |id, parent = nil|
-    ^super.newCopyArgs(id).init;
+    ^super.newCopyArgs(id, parent).init;
   }
 
   quat2euler { |quat|
-    var w = quat.a, x = quat.b, y = quat.c, z = quat.d,
+    // Quaternion
+    //var w = quat.a, x = quat.b, y = quat.c, z = quat.d,
+    //var q = [quat.b, quat.c, quat.d, quat.a],
+    var q = [quat.a, quat.b, quat.c, quat.d],
+    // Euler angles; roll, pitch and yaw
+    e = [0, 0, 0];
 
-    x2 = x + x,  y2 = y + y,  z2 = z + z,
+    /**/
-    xx = x * x2, xy = x * y2, xz = x * z2,
+    e[0] = atan2( (2 * q[1] * q[2]) - (2 * q[0] * q[3]), (2 * q[0]*q[0]) + (2 * q[1] * q[1]) - 1); // psi
-    yy = y * y2, yz = y * z2, zz = z * z2,
+    e[1] = asin( (2 * q[1] * q[3]) + (2 * q[0] * q[2])).neg; // theta
-    wx = w * x2, wy = w * y2, wz = w * z2,
+    e[2] = atan2( (2 * q[2] * q[3]) - (2 * q[0] * q[1]), (2 * q[0] * q[0]) + (2 * q[3] * q[3]) - 1); // phi
+    /**/
+
+    /*
+    // Variables
+    sinr_cosp, cosr_cosp, sinp, cosp, siny_cosp, cosy_cosp;
+    // Conversion source:
+    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code_2
+
+    sinr_cosp = 2 * (w * x * y * z);
+    cosr_cosp = 1 - (2 * (x * x * y * y));
+    // roll (x-axis rotation)
+    e[0] = atan2(sinr_cosp, cosr_cosp);
+
+    sinp = sqrt(1 + (2 * ((w * y) - (x * z))));
+    cosp = sqrt(1 - (2 * ((w * y) - (x * z))));
+    // pitch (y-axis rotation)
+    e[1] = 2 * atan2(sinp, cosp) - 0.5pi;
+
+    siny_cosp = 2 * ((w * z) + (x * y));
+    cosy_cosp = 1 - (2 * ((y * y) + (z * z)));
+    // yaw (z-axis rotation)
+    e[2] = atan2(siny_cosp, cosy_cosp);
+    */
+
+    /*
+    x2, y2, z2, xx, xy, xz, yy, yz, zz, wx, wy, wz,
+    matrix, m11, m12, m13, m21, m22, m23, m31, m32, m33;
+
+    x2 = x + x;  y2 = y + y;  z2 = z + z;
+    xx = x * x2; xy = x * y2; xz = x * z2;
+    yy = y * y2; yz = y * z2; zz = z * z2;
+    wx = w * x2; wy = w * y2; wz = w * z2;
 
     matrix = [
       1 - ( yy + zz ), xy + wz,         xz - wy,         0,
@@ -46,11 +97,9 @@ AHRSensor {
       0,               0,               0,               1
     ];
 
-    var m11 = matrix[0], m12 = matrix[4], m13 = matrix[8];
+    m11 = matrix[0]; m12 = matrix[4]; m13 = matrix[8];
-    var m21 = matrix[1], m22 = matrix[5], m23 = matrix[9];
+    m21 = matrix[1]; m22 = matrix[5]; m23 = matrix[9];
-    var m31 = matrix[2], m32 = matrix[6], m33 = matrix[10];
+    m31 = matrix[2]; m32 = matrix[6]; m33 = matrix[10];
-
-    var e = [0, 0, 0];
 
     e[1] = asin(m13.clip(-1, 1));
 
@@ -61,13 +110,14 @@ AHRSensor {
         e[0] = atan2(m32, m22);
         e[2] = 0;
     });
+    */
 
     ^e;
   }
 
   init {
 	quat = Quaternion;
-	calibrationQuat = Quaternion;
+	calibQuat = Quaternion.new(1, 0, 0, 0);
 	euler = [0, 0, 0];
 	accel = [0, 0, 0];
 	battery = 0;
@@ -91,32 +141,47 @@ AHRSensor {
 	gEy = StaticText().string_(0).stringColor_(cGreen);
 	gEz = StaticText().string_(0).stringColor_(cBlue);
 
+    // Razlika v eulerju
+	gEdx = StaticText().string_(0).stringColor_(cRed);
+	gEdy = StaticText().string_(0).stringColor_(cGreen);
+	gEdz = StaticText().string_(0).stringColor_(cBlue);
+
 	//gAx = StaticText().string_(0).stringColor_(cRed);
 	//gAy = StaticText().string_(0).stringColor_(cGreen);
 	//gAz = StaticText().string_(0).stringColor_(cBlue);
     gAx = LevelIndicator();
     gAy = LevelIndicator();
     gAz = LevelIndicator();
+    gAs = LevelIndicator();
 
 	gQw = StaticText().string_(0).stringColor_(cPurple);
 	gQx = StaticText().string_(0).stringColor_(cRed);
 	gQy = StaticText().string_(0).stringColor_(cGreen);
 	gQz = StaticText().string_(0).stringColor_(cBlue);
 
+	gQc = Button().string_("cal").action_({ |butt|
+      this.calibrateQuat;
+    });
+
 	gB = StaticText().string_(0).stringColor_(cRed);
+	gBi = LevelIndicator();
 	gEps = StaticText().string_(0);
   }
 
   getGui {
-	^[
+    var napis = "Sensor " ++ id, elementi;
+    if (parent != nil) { napis = napis ++ " (parent " ++ parent ++ ")"};
+
+	elementi = [
       [
-        StaticText().font_(Font("OpenSans", 12, true)).string_("Sensor " ++ id),
+        StaticText().font_(Font("OpenSans", 12, true)).string_(napis),
-        nil,
         nil,
         [StaticText().string_("bat: "), align: \right],
         gB,
+        gBi,
         [StaticText().string_("events/s: "), align: \right],
-        gEps
+        gEps,
+        gQc
       ],
 	  [
 		StaticText().string_("quaternion: "),
@@ -146,14 +211,34 @@ AHRSensor {
 		gAy,
 		[StaticText().string_("z: "), align: \right],
 		gAz,
+		[StaticText().string_("sum: "), align: \right],
+        gAs
 	  ],
-      []
+      if (parent != nil) {
+        [ 
+          StaticText().string_("euler d: "),
+          [StaticText().string_("x: "), align: \right],
+          gEdx,
+          [StaticText().string_("y: "), align: \right],
+          gEdy,
+          [StaticText().string_("z: "), align: \right],
+          gEdz,
        ];
       }
+	];
+    ^elementi;
+  }
+
+  calibrateQuat {
+    calibQuat = quat;
+  }
 
   updateEuler { |newQuat|
-    var quatDiff = newQuat / quat;
+    euler = this.quat2euler(newQuat);
-    euler = euler + this.quat2euler(quatDiff);
+  }
+
+  updateEulerD { |newQuat|
+    eulerD = this.quat2euler(newQuat);
   }
 
   refreshGuiQuat {
@@ -172,6 +257,13 @@ AHRSensor {
 	gEy.string_(euler[1].asStringPrec(prec));
 	gEz.string_(euler[2].asStringPrec(prec));
   }
+  refreshGuiEulerD {
+    // Stevilo decimalk
+    var prec = 3;
+	gEdx.string_(eulerD[0].asStringPrec(prec));
+	gEdy.string_(eulerD[1].asStringPrec(prec));
+	gEdz.string_(eulerD[2].asStringPrec(prec));
+  }
 
   refreshGuiAccel {
     // Stevilo decimalk
@@ -183,12 +275,14 @@ AHRSensor {
     [gAx, gAy, gAz].do({|el, i|
       el.value_(accel[i].linlin(from, to, 0, 1));
     });
+    gAs.value_(accelSum.linlin(0, 100, 0, 1));
   }
 
   refreshGuiBat {
     // Stevilo decimalk
     var prec = 2;
 	gB.string_(battery.asStringPrec(prec));
+    gBi.value_(battery.linlin(3.6, 4.2, 0, 1));
   }
 
   refreshGuiEps {

linux-receiver/main.c

@@ -35,7 +35,7 @@ $ sudo ifconfig wlp5s0 up
 #include <linux/filter.h>
 
 // Debug?
-#define DEBUG
+//#define DEBUG
 
 // Booleans
 #include <stdbool.h>
@@ -59,7 +59,7 @@ lo_address osc_dest;
 // Receiver MAC start at byte 52
 #define WLAN_DA_OFFSET 52
 /*our MAC address*/
-uint8_t sprejemnikMac[] = { 0x9c, 0xb6, 0xd0, 0xc4, 0xe8, 0xb9 };
+uint8_t sprejemnikMac[] = { 0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C };
 uint8_t wlan_da[] = { 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 };
 
 // ESPNOW packet identifier
@@ -183,7 +183,6 @@ void print_packet(uint8_t *data, int len) {
   char glava[32];
   lo_bundle svezenj;
   lo_message m;
-  char* sporocilo;
   size_t dolzina;
 
   for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
@@ -198,8 +197,6 @@ void print_packet(uint8_t *data, int len) {
       lo_message_add_float(m, odcitki[i].aY);
       lo_message_add_float(m, odcitki[i].aZ);
 
-      //free(sporocilo);
-
       lo_bundle_add_message(svezenj, glava, m);
       sprintf(glava, "/ww/%d/quat", i); 
       m = lo_message_new();
@@ -210,14 +207,12 @@ void print_packet(uint8_t *data, int len) {
 
       lo_bundle_add_message(svezenj, glava, m);
 
-      sporocilo = lo_bundle_serialise(svezenj, NULL, &dolzina);
+#ifdef DEBUG
       lo_bundle_pp(svezenj);
-      lo_send_bundle(osc_dest, svezenj);
+#endif
-      printf("%s\n", sporocilo);
-      lo_bundle_free(svezenj);
 
-      //printf("%s\n", glava);
+      lo_send_bundle(osc_dest, svezenj);
-      //free(sporocilo);
+      lo_bundle_free(svezenj);
     }
   }
 }
@@ -253,17 +248,17 @@ int create_raw_socket(char *dev, struct sock_fprog *bpf)
 }
 
 int main(int argc, char **argv) {
-  assert(argc == 2);
+  assert(argc == 3);
 
   uint8_t buff[MAX_PACKET_LEN] = {0};
   int sock_fd;
   char *dev = argv[1];
+  char *scport = argv[2];
   struct sock_fprog bpf = {FILTER_LENGTH, bpfcode};
 
   sock_fd = create_raw_socket(dev, &bpf); /* Creating the raw socket */
 
-  // @TODO get this from args?
+  osc_dest = lo_address_new("localhost", scport);
-  osc_dest = lo_address_new("localhost", "57121");
 
   printf("\n Waiting to receive packets ........ \n");
   gettimeofday(&cas, NULL);

linux-receiver/pozeni.sh

@@ -1,5 +1,6 @@
 #!/bin/bash
 
 dev=${1:-wlp3s0}
+oscport=${1:-57120}
 
-sudo ./bin/receiver $dev
+sudo ./bin/receiver $dev $oscport

platformio.ini

@@ -21,6 +21,10 @@ build_flags = -D ARDUINO_USB_CDC_ON_BOOT=1
 build_src_filter = 
 	+<main.cpp>
 
+[env:calibration]
+build_src_filter = 
+	+<calibration.cpp>
+
 ;; Olimex prototype sketch
 [env:main-olimex]
 build_src_filter = 

src/calibration.cpp

@@ -0,0 +1,310 @@
+#include <Arduino.h>
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <SPI.h>
+#include <Adafruit_BNO055.h>
+#include <utility/imumaths.h>
+#include <EEPROM.h>
+
+/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor),
+   which provides a common 'type' for sensor data and some helper functions.
+
+   To use this driver you will also need to download the Adafruit_Sensor
+   library and include it in your libraries folder.
+
+   You should also assign a unique ID to this sensor for use with
+   the Adafruit Sensor API so that you can identify this particular
+   sensor in any data logs, etc.  To assign a unique ID, simply
+   provide an appropriate value in the constructor below (12345
+   is used by default in this example).
+
+   Connections
+   ===========
+   Connect SCL to analog 5
+   Connect SDA to analog 4
+   Connect VDD to 3-5V DC
+   Connect GROUND to common ground
+
+   History
+   =======
+   2015/MAR/03  - First release (KTOWN)
+   2015/AUG/27  - Added calibration and system status helpers
+   2015/NOV/13  - Added calibration save and restore
+   */
+
+/* Set the delay between fresh samples */
+#define BNO055_SAMPLERATE_DELAY_MS (100)
+
+// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
+//                                   id, address
+Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
+
+/**************************************************************************/
+/*
+    Displays some basic information on this sensor from the unified
+    sensor API sensor_t type (see Adafruit_Sensor for more information)
+    */
+/**************************************************************************/
+void displaySensorDetails(void)
+{
+    sensor_t sensor;
+    bno.getSensor(&sensor);
+    Serial.println("------------------------------------");
+    Serial.print("Sensor:       "); Serial.println(sensor.name);
+    Serial.print("Driver Ver:   "); Serial.println(sensor.version);
+    Serial.print("Unique ID:    "); Serial.println(sensor.sensor_id);
+    Serial.print("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" xxx");
+    Serial.print("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" xxx");
+    Serial.print("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" xxx");
+    Serial.println("------------------------------------");
+    Serial.println("");
+    delay(500);
+}
+
+/**************************************************************************/
+/*
+    Display some basic info about the sensor status
+    */
+/**************************************************************************/
+void displaySensorStatus(void)
+{
+    /* Get the system status values (mostly for debugging purposes) */
+    uint8_t system_status, self_test_results, system_error;
+    system_status = self_test_results = system_error = 0;
+    bno.getSystemStatus(&system_status, &self_test_results, &system_error);
+
+    /* Display the results in the Serial Monitor */
+    Serial.println("");
+    Serial.print("System Status: 0x");
+    Serial.println(system_status, HEX);
+    Serial.print("Self Test:     0x");
+    Serial.println(self_test_results, HEX);
+    Serial.print("System Error:  0x");
+    Serial.println(system_error, HEX);
+    Serial.println("");
+    delay(500);
+}
+
+/**************************************************************************/
+/*
+    Display sensor calibration status
+    */
+/**************************************************************************/
+void displayCalStatus(void)
+{
+    /* Get the four calibration values (0..3) */
+    /* Any sensor data reporting 0 should be ignored, */
+    /* 3 means 'fully calibrated" */
+    uint8_t system, gyro, accel, mag;
+    system = gyro = accel = mag = 0;
+    bno.getCalibration(&system, &gyro, &accel, &mag);
+
+    /* The data should be ignored until the system calibration is > 0 */
+    Serial.print("\t");
+    if (!system)
+    {
+        Serial.print("! ");
+    }
+
+    /* Display the individual values */
+    Serial.print("Sys:");
+    Serial.print(system, DEC);
+    Serial.print(" G:");
+    Serial.print(gyro, DEC);
+    Serial.print(" A:");
+    Serial.print(accel, DEC);
+    Serial.print(" M:");
+    Serial.print(mag, DEC);
+}
+
+/**************************************************************************/
+/*
+    Display the raw calibration offset and radius data
+    */
+/**************************************************************************/
+void displaySensorOffsets(const adafruit_bno055_offsets_t &calibData)
+{
+    Serial.print("Accelerometer: ");
+    Serial.print(calibData.accel_offset_x); Serial.print(" ");
+    Serial.print(calibData.accel_offset_y); Serial.print(" ");
+    Serial.print(calibData.accel_offset_z); Serial.print(" ");
+
+    Serial.print("\nGyro: ");
+    Serial.print(calibData.gyro_offset_x); Serial.print(" ");
+    Serial.print(calibData.gyro_offset_y); Serial.print(" ");
+    Serial.print(calibData.gyro_offset_z); Serial.print(" ");
+
+    Serial.print("\nMag: ");
+    Serial.print(calibData.mag_offset_x); Serial.print(" ");
+    Serial.print(calibData.mag_offset_y); Serial.print(" ");
+    Serial.print(calibData.mag_offset_z); Serial.print(" ");
+
+    Serial.print("\nAccel Radius: ");
+    Serial.print(calibData.accel_radius);
+
+    Serial.print("\nMag Radius: ");
+    Serial.print(calibData.mag_radius);
+}
+
+
+/**************************************************************************/
+/*
+    Arduino setup function (automatically called at startup)
+    */
+/**************************************************************************/
+
+int eeprom_size;
+
+void setup(void)
+{
+    Serial.begin(115200);
+    delay(8000);
+    Serial.println("Orientation Sensor Test"); Serial.println("");
+
+    /* Initialise the sensor */
+    if (!bno.begin())
+    {
+        /* There was a problem detecting the BNO055 ... check your connections */
+        Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+        while (1);
+    }
+
+    // EEPROM init
+    eeprom_size = sizeof(long) + sizeof(adafruit_bno055_offsets_t);
+    EEPROM.begin(eeprom_size);
+
+    Serial.print("EEPROM init (len ");
+    Serial.print(EEPROM.length());
+    Serial.println(")\n");
+
+    int eeAddress = 0;
+    long bnoID;
+    bool foundCalib = false;
+
+    EEPROM.get(eeAddress, bnoID);
+
+    adafruit_bno055_offsets_t calibrationData;
+    sensor_t sensor;
+
+    /*
+    *  Look for the sensor's unique ID at the beginning oF EEPROM.
+    *  This isn't foolproof, but it's better than nothing.
+    */
+    bno.getSensor(&sensor);
+    if (bnoID != sensor.sensor_id)
+    {
+        Serial.println("\nNo Calibration Data for this sensor exists in EEPROM");
+        delay(500);
+    }
+    else
+    {
+        Serial.println("\nFound Calibration for this sensor in EEPROM.");
+        eeAddress += sizeof(long);
+        EEPROM.get(eeAddress, calibrationData);
+
+        displaySensorOffsets(calibrationData);
+
+        Serial.println("\n\nRestoring Calibration data to the BNO055...");
+        bno.setSensorOffsets(calibrationData);
+
+        Serial.println("\n\nCalibration data loaded into BNO055");
+        foundCalib = true;
+    }
+
+    delay(1000);
+
+    /* Display some basic information on this sensor */
+    displaySensorDetails();
+
+    /* Optional: Display current status */
+    displaySensorStatus();
+
+   /* Crystal must be configured AFTER loading calibration data into BNO055. */
+    bno.setExtCrystalUse(true);
+
+    sensors_event_t event;
+    bno.getEvent(&event);
+    /* always recal the mag as It goes out of calibration very often */
+    if (foundCalib){
+        Serial.println("Move sensor slightly to calibrate magnetometers");
+        while (!bno.isFullyCalibrated())
+        {
+            bno.getEvent(&event);
+            delay(BNO055_SAMPLERATE_DELAY_MS);
+        }
+    }
+    else
+    {
+        Serial.println("Please Calibrate Sensor: ");
+        while (!bno.isFullyCalibrated())
+        {
+            bno.getEvent(&event);
+
+            Serial.print("X: ");
+            Serial.print(event.orientation.x, 4);
+            Serial.print("\tY: ");
+            Serial.print(event.orientation.y, 4);
+            Serial.print("\tZ: ");
+            Serial.print(event.orientation.z, 4);
+
+            /* Optional: Display calibration status */
+            displayCalStatus();
+
+            /* New line for the next sample */
+            Serial.println("");
+
+            /* Wait the specified delay before requesting new data */
+            delay(BNO055_SAMPLERATE_DELAY_MS);
+        }
+    }
+
+    Serial.println("\nFully calibrated!");
+    Serial.println("--------------------------------");
+    Serial.println("Calibration Results: ");
+    adafruit_bno055_offsets_t newCalib;
+    bno.getSensorOffsets(newCalib);
+    displaySensorOffsets(newCalib);
+
+    Serial.println("\n\nStoring calibration data to EEPROM...");
+
+    eeAddress = 0;
+    bno.getSensor(&sensor);
+    bnoID = sensor.sensor_id;
+
+    EEPROM.put(eeAddress, bnoID);
+
+    eeAddress += sizeof(long);
+    EEPROM.put(eeAddress, newCalib);
+    EEPROM.commit();
+    Serial.println("Data stored to EEPROM.");
+
+    Serial.println("\n--------------------------------\n");
+    delay(500);
+    while (1) {};
+}
+
+void loop() {
+    /* Get a new sensor event */
+    sensors_event_t event;
+    bno.getEvent(&event);
+
+    /* Display the floating point data */
+    Serial.print("X: ");
+    Serial.print(event.orientation.x, 4);
+    Serial.print("\tY: ");
+    Serial.print(event.orientation.y, 4);
+    Serial.print("\tZ: ");
+    Serial.print(event.orientation.z, 4);
+
+    /* Optional: Display calibration status */
+    displayCalStatus();
+
+    /* Optional: Display sensor status (debug only) */
+    //displaySensorStatus();
+
+    /* New line for the next sample */
+    Serial.println("");
+
+    /* Wait the specified delay before requesting new data */
+    delay(BNO055_SAMPLERATE_DELAY_MS);
+}

src/main.cpp

@@ -1,9 +1,10 @@
 #include <Arduino.h>
 #include <Wire.h>
 #include "esp_adc_cal.h"
+#include <EEPROM.h>
 
 // ID senzorja mora bit unikaten! (se poslje poleg parametrov)
-#define SENSOR_ID 5
+#define SENSOR_ID 6
 
 // Stanje baterije
 #define BATTERY_PIN 2
@@ -35,6 +36,8 @@
 
 // MAC naslov sprejemnika
 uint8_t sprejemnikMac[] = { 0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C };
+
+// Comp
 sensor_msg odcitek;
 esp_now_peer_info_t peerInfo;
 
@@ -47,6 +50,11 @@ imu::Vector<3> linearAccel;
 
 int cas = 0;
 
+// EEPROM za kalibracijo
+int eeprom_size;
+/* Set the delay between fresh samples (calibration) */
+#define BNO055_SAMPLERATE_DELAY_MS (100)
+
 void error_blink() {
   while(1) {
     digitalWrite(LED_PIN, LOW);
@@ -75,20 +83,6 @@ void setup() {
     delay(500);
   }
 
-  bno = Adafruit_BNO055(55, 0x28, &Wire);
-  /* Initialise the sensor */
-  if(!bno.begin(OPERATION_MODE_NDOF)) {
-  //if(!bno.begin(OPERATION_MODE_AMG)) {
-    /* There was a problem detecting the BNO055 ... check your connections */
-    Serial.println("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
-    error_blink();
-  }
-
-  delay(1000);
-
-  /* Use external crystal for better accuracy? */
-  bno.setExtCrystalUse(true);
-
   // WIFI init
   WiFi.mode(WIFI_STA);
   if (esp_now_init() != ESP_OK) {
@@ -105,6 +99,77 @@ void setup() {
     error_blink();
   }
 
+  bno = Adafruit_BNO055(55, 0x28, &Wire);
+
+  if (!bno.begin(OPERATION_MODE_NDOF)) {
+    /* There was a problem detecting the BNO055 ... check your connections */
+    Serial.println("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+    error_blink();
+  }
+
+  // EEPROM init
+  eeprom_size = sizeof(long) + sizeof(adafruit_bno055_offsets_t);
+  EEPROM.begin(eeprom_size);
+  Serial.print("EEPROM init (len ");
+  Serial.print(EEPROM.length());
+  Serial.println(")\n");
+
+  // Fetch calibration
+  int eeAddress = 0;
+  long bnoID;
+  bool foundCalib = false;
+
+  EEPROM.get(eeAddress, bnoID);
+
+  adafruit_bno055_offsets_t calibrationData;
+  sensor_t sensor;
+
+  /*
+   *  Look for the sensor's unique ID at the beginning oF EEPROM.
+   *  This isn't foolproof, but it's better than nothing.
+   */
+  bno.getSensor(&sensor);
+  if (bnoID != sensor.sensor_id) {
+    Serial.println("\nNo Calibration Data for this sensor exists in EEPROM");
+    delay(500);
+  } else {
+    Serial.println("\nFound Calibration for this sensor in EEPROM.");
+    eeAddress += sizeof(long);
+    EEPROM.get(eeAddress, calibrationData);
+
+    Serial.println("\n\nRestoring Calibration data to the BNO055...");
+    bno.setSensorOffsets(calibrationData);
+
+    Serial.println("\n\nCalibration data loaded into BNO055");
+  }
+
+  delay(1000);
+
+  /* Use external crystal for better accuracy? */
+  /* Crystal must be configured AFTER loading calibration data into BNO055. */
+  bno.setExtCrystalUse(true);
+
+  delay(1000);
+
+  sensors_event_t event;
+  bno.getEvent(&event);
+
+  /* always recal the mag as It goes out of calibration very often */
+  if (foundCalib){
+    Serial.println("Move sensor slightly to calibrate magnetometers");
+    while (!bno.isFullyCalibrated()) {
+      bno.getEvent(&event);
+      delay(BNO055_SAMPLERATE_DELAY_MS);
+    }
+  }
+
+  delay(1000);
+
+  /* Change mode to sensor fusion */
+  bno.setMode(OPERATION_MODE_NDOF);
+
+  delay(1000);
+
   // Running - led on!
   digitalWrite(LED_PIN, HIGH);
 

src/sprejemnik.cpp

@@ -65,8 +65,9 @@ void prejemPodatkov(const uint8_t * mac_addr, const uint8_t * noviPodatki, int l
 
 void setup() {
   // Nizja CPU frekvenca
-  setCpuFrequencyMhz(80);
+  //setCpuFrequencyMhz(80);
-  SLIPSerial.begin(115200);
+  //SLIPSerial.begin(115200);
+  SLIPSerial.begin(230400);
 
 
   // Ne posiljaj preden se podatki napolnijo

utopia.scd

@@ -1,12 +1,19 @@
 // Dependencies:
 //  - SLIPDecoder (https://git.kompot.si/g1smo/SLIPDecoder)
 //  - MathLib (https://depts.washington.edu/dxscdoc/Help/Browse.html#Libraries%3EMathLib)
+//  - OSCRecorder
 
+Quarks.install("https://github.com/cappelnord/OSCRecorder.git");
+
+OSCRecorderGUI();
+
+NetAddr.langPort;
+OSCFunc.trace(true);
 (
 
 // Initialize the the receiver via SLIP decoder
-~receiverPath = "/dev/ttyUSB0";
+~receiverPath = "/dev/ttyACM0";
-~baudRate = 115200;
+~baudRate = 230400;
 
 OSCFunc.trace(true); // debug osc
 OSCFunc.trace(false);
@@ -17,30 +24,50 @@ OSCFunc.trace(false);
 
 // Indeksirani so z IDjem
 ~senzorji = Dictionary.new;
+// Indeksirani so s parent IDjem
+~starsi = Dictionary.new;
 
 // Dodaj senzorje
-((1..5)).do({ |id|
+// Pari: parent/child; core nima parenta, ostali pa imajo "relativno rotacijo", ki je odvisna od jedra
+
+~razmerja = [
+  // Core
+  [1, nil],
+  //[6, 1],
+  [6, 1],
+  [3, 6],
+  [7, nil]
+].do({ |par|
+  var s = AHRSensor.new(par[0], par[1]);
+  ~senzorji.put(par[0], s);
+  ~starsi.put(par[1], s);
+});
+/*
+~senzorji.put(1, AHRSensor.new(1));
+~senzorji.put(2, AHRSensor.new(2));
+// Ostali (3 do 6)
+((3..6)).do({ |id|
   ~senzorji.put(id, AHRSensor.new(id));
 });
+  */
 
 // Olimex test
-~senzorji.put(9, AHRSensor.new(9));
+//~senzorji.put(9, AHRSensor.new(9));
 
 //~senzorji.postln;
 
 ~decoder = SLIPDecoder.new(~receiverPath);
-~w = Window.new("Utopia || C²", Rect(300, 300, 600, ~senzorji.size * 20),true);
+~w = Window.new("Utopia || C²", Rect(300, ~senzorji.size * 60, 600, ~senzorji.size * 100),true);
-~ttyInput = TextField().string_(~receiverPath);
+
+// Midi naprava
+MIDIClient.init;
+MIDIClient.destinations;
+~midi = MIDIOut.new(0);
+~midi.connect(1);
 
 ~elementi = ~senzorji.values.sort({ |a, b| a.id < b.id;}).collect({|s| s.getGui;});
 
-~w.layout_(
+~startStopDecoder = { |butt|
-  VLayout(
-	HLayout(
-      // HEADER; serial path, buttons
-	  StaticText().string_("Serial path: "),
-	  ~ttyInput,
-	  Button().string_("Start").action_({ |butt|
   if ((~decoder.running.not), {
     // If not running, start decoder
     ~decoder.trace(true); // debug slip decoder
@@ -51,14 +78,22 @@ OSCFunc.trace(false);
     // Else stop the decoder
     ~decoder.stop;
     butt.string_("Start")
-		});
+  };
-	  })
+  );
+};
+
+~ttyInput = TextField().string_(~receiverPath).keyDownAction_({ |input, char| if (char == $\r, { ~startStopDecoder.value(~ttyInput); }); });
+
+~w.layout_(
+  VLayout(
+	HLayout(
+      // HEADER; serial path, buttons
+	  StaticText().string_("Serial path: "),
+	  ~ttyInput,
+	  Button().string_("Start").action_(~startStopDecoder);
 	),
-    [],
     // Sensor rows
-    GridLayout.rows(
+    GridLayout.rows(*~elementi.flatten)
-      *~elementi.flatten
-    )
   )
 );
 
@@ -76,18 +111,27 @@ OSCFunc.trace(false);
   // Quat listener
   q = OSCdef.new((\quat ++ s.id), { |msg, time, addr, recvPort|
     var q;
+    // We get X Y Z W, but supercollider has W X Y Z!
     q = Quaternion.new(msg[4], msg[1], msg[2], msg[3]);
 
     // Count quat events
     Routine {
+      var quatD;
+
       senzor.eps = senzor.eps + 1;
       senzor.quat = q;
       senzor.refreshGuiQuat;
 
-      senzor.updateEuler(q);
+      //quatD = senzor.calibQuat.conjugate * q;
+      quatD = senzor.calibQuat.reciprocal * q;
+      senzor.updateEuler(quatD);
       senzor.refreshGuiEuler;
-      senzor.euler.postln;
 
+      // Relativni euler koti (glede na starsa)
+      if (senzor.parent != nil) {
+        senzor.updateEulerD(q);
+        senzor.refreshGuiEulerD;
+      };
     }.play(AppClock);
 
   }, oscHeader ++ "/quat");
@@ -97,7 +141,17 @@ OSCFunc.trace(false);
     var a;
     a = msg.at((1..3));
     Routine {
+      var vectorSum = 0, xy = 0, xyz = 0;
+
       senzor.accel = a;
+
+      // Vsota vektorjev:
+      xy = sqrt((a[0] * a[0]) + (a[1] * a[1]));
+      xyz = sqrt((xy * xy) + (a[2] * a[2]));
+      senzor.accelSum = xyz;
+
+      ~midi.control(s.id, 1, xyz);
+
       senzor.refreshGuiAccel;
     }.play(AppClock);
   }, oscHeader ++ "/acc");