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

posodobitve - vec gui-ja, euler pretvorbe, ...
main
Jurij Podgoršek 2024-09-18 13:56:49 +02:00
parent b51daa1b00
commit 8ca3b235b7
9 changed files with 606 additions and 82 deletions

View File

@ -1,9 +1,15 @@
AHRSensor { AHRSensor {
var <id, var <id,
<>calibrationQuat, <parent,
// Kalibracijski quaternion (kao nula)
<>calibQuat,
<>quat, <>quat,
<>euler, <>euler,
<>eulerD,
<>accel, <>accel,
<>accelSum,
<>battery, <>battery,
<>eps, <>eps,
@ -14,30 +20,75 @@ AHRSensor {
<gQx, <gQx,
<gQy, <gQy,
<gQz, <gQz,
// Kalibriraj quaternion
<gQc,
// Eulerjevi koti // Eulerjevi koti
<gEx, <gEx,
<gEy, <gEy,
<gEz, <gEz,
// Relativni eulerjevi koti
<gEdx,
<gEdy,
<gEdz,
// Pospeskomer // Pospeskomer
<gAx, <gAx,
<gAy, <gAy,
<gAz, <gAz,
<gAs,
// Baterija // Baterija
<gB, <gB,
<gBi,
// Dogodkov na sekundo // Dogodkov na sekundo
<gEps; <gEps;
*new { |id| *new { |id, parent = nil|
^super.newCopyArgs(id).init; ^super.newCopyArgs(id, parent).init;
} }
quat2euler { |quat| 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 = [ matrix = [
1 - ( yy + zz ), xy + wz, xz - wy, 0, 1 - ( yy + zz ), xy + wz, xz - wy, 0,
@ -46,13 +97,11 @@ AHRSensor {
0, 0, 0, 1 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));
e[1] = asin(m13.clip(- 1, 1));
if ((m13.abs < 0.9999999), { if ((m13.abs < 0.9999999), {
e[0] = atan2(m23.neg, m33); e[0] = atan2(m23.neg, m33);
@ -61,13 +110,14 @@ AHRSensor {
e[0] = atan2(m32, m22); e[0] = atan2(m32, m22);
e[2] = 0; e[2] = 0;
}); });
*/
^e; ^e;
} }
init { init {
quat = Quaternion; quat = Quaternion;
calibrationQuat = Quaternion; calibQuat = Quaternion.new(1, 0, 0, 0);
euler = [0, 0, 0]; euler = [0, 0, 0];
accel = [0, 0, 0]; accel = [0, 0, 0];
battery = 0; battery = 0;
@ -91,32 +141,47 @@ AHRSensor {
gEy = StaticText().string_(0).stringColor_(cGreen); gEy = StaticText().string_(0).stringColor_(cGreen);
gEz = StaticText().string_(0).stringColor_(cBlue); 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); //gAx = StaticText().string_(0).stringColor_(cRed);
//gAy = StaticText().string_(0).stringColor_(cGreen); //gAy = StaticText().string_(0).stringColor_(cGreen);
//gAz = StaticText().string_(0).stringColor_(cBlue); //gAz = StaticText().string_(0).stringColor_(cBlue);
gAx = LevelIndicator(); gAx = LevelIndicator();
gAy = LevelIndicator(); gAy = LevelIndicator();
gAz = LevelIndicator(); gAz = LevelIndicator();
gAs = LevelIndicator();
gQw = StaticText().string_(0).stringColor_(cPurple); gQw = StaticText().string_(0).stringColor_(cPurple);
gQx = StaticText().string_(0).stringColor_(cRed); gQx = StaticText().string_(0).stringColor_(cRed);
gQy = StaticText().string_(0).stringColor_(cGreen); gQy = StaticText().string_(0).stringColor_(cGreen);
gQz = StaticText().string_(0).stringColor_(cBlue); gQz = StaticText().string_(0).stringColor_(cBlue);
gQc = Button().string_("cal").action_({ |butt|
this.calibrateQuat;
});
gB = StaticText().string_(0).stringColor_(cRed); gB = StaticText().string_(0).stringColor_(cRed);
gBi = LevelIndicator();
gEps = StaticText().string_(0); gEps = StaticText().string_(0);
} }
getGui { 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, nil,
[StaticText().string_("bat: "), align: \right], [StaticText().string_("bat: "), align: \right],
gB, gB,
gBi,
[StaticText().string_("events/s: "), align: \right], [StaticText().string_("events/s: "), align: \right],
gEps gEps,
gQc
], ],
[ [
StaticText().string_("quaternion: "), StaticText().string_("quaternion: "),
@ -146,14 +211,34 @@ AHRSensor {
gAy, gAy,
[StaticText().string_("z: "), align: \right], [StaticText().string_("z: "), align: \right],
gAz, 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| updateEuler { |newQuat|
var quatDiff = newQuat / quat; euler = this.quat2euler(newQuat);
euler = euler + this.quat2euler(quatDiff); }
updateEulerD { |newQuat|
eulerD = this.quat2euler(newQuat);
} }
refreshGuiQuat { refreshGuiQuat {
@ -172,6 +257,13 @@ AHRSensor {
gEy.string_(euler[1].asStringPrec(prec)); gEy.string_(euler[1].asStringPrec(prec));
gEz.string_(euler[2].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 { refreshGuiAccel {
// Stevilo decimalk // Stevilo decimalk
@ -183,12 +275,14 @@ AHRSensor {
[gAx, gAy, gAz].do({|el, i| [gAx, gAy, gAz].do({|el, i|
el.value_(accel[i].linlin(from, to, 0, 1)); el.value_(accel[i].linlin(from, to, 0, 1));
}); });
gAs.value_(accelSum.linlin(0, 100, 0, 1));
} }
refreshGuiBat { refreshGuiBat {
// Stevilo decimalk // Stevilo decimalk
var prec = 2; var prec = 2;
gB.string_(battery.asStringPrec(prec)); gB.string_(battery.asStringPrec(prec));
gBi.value_(battery.linlin(3.6, 4.2, 0, 1));
} }
refreshGuiEps { refreshGuiEps {

Binary file not shown.

View File

@ -35,7 +35,7 @@ $ sudo ifconfig wlp5s0 up
#include <linux/filter.h> #include <linux/filter.h>
// Debug? // Debug?
#define DEBUG //#define DEBUG
// Booleans // Booleans
#include <stdbool.h> #include <stdbool.h>
@ -59,7 +59,7 @@ lo_address osc_dest;
// Receiver MAC start at byte 52 // Receiver MAC start at byte 52
#define WLAN_DA_OFFSET 52 #define WLAN_DA_OFFSET 52
/*our MAC address*/ /*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 }; uint8_t wlan_da[] = { 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 };
// ESPNOW packet identifier // ESPNOW packet identifier
@ -183,7 +183,6 @@ void print_packet(uint8_t *data, int len) {
char glava[32]; char glava[32];
lo_bundle svezenj; lo_bundle svezenj;
lo_message m; lo_message m;
char* sporocilo;
size_t dolzina; size_t dolzina;
for (int i = 0; i < ST_SPREJEMNIKOV; i++) { 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].aY);
lo_message_add_float(m, odcitki[i].aZ); lo_message_add_float(m, odcitki[i].aZ);
//free(sporocilo);
lo_bundle_add_message(svezenj, glava, m); lo_bundle_add_message(svezenj, glava, m);
sprintf(glava, "/ww/%d/quat", i); sprintf(glava, "/ww/%d/quat", i);
m = lo_message_new(); m = lo_message_new();
@ -210,14 +207,12 @@ void print_packet(uint8_t *data, int len) {
lo_bundle_add_message(svezenj, glava, m); lo_bundle_add_message(svezenj, glava, m);
sporocilo = lo_bundle_serialise(svezenj, NULL, &dolzina); #ifdef DEBUG
lo_bundle_pp(svezenj); 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) { int main(int argc, char **argv) {
assert(argc == 2); assert(argc == 3);
uint8_t buff[MAX_PACKET_LEN] = {0}; uint8_t buff[MAX_PACKET_LEN] = {0};
int sock_fd; int sock_fd;
char *dev = argv[1]; char *dev = argv[1];
char *scport = argv[2];
struct sock_fprog bpf = {FILTER_LENGTH, bpfcode}; struct sock_fprog bpf = {FILTER_LENGTH, bpfcode};
sock_fd = create_raw_socket(dev, &bpf); /* Creating the raw socket */ 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"); printf("\n Waiting to receive packets ........ \n");
gettimeofday(&cas, NULL); gettimeofday(&cas, NULL);

View File

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

View File

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

310
src/calibration.cpp 100644
View File

@ -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);
}

View File

@ -1,9 +1,10 @@
#include <Arduino.h> #include <Arduino.h>
#include <Wire.h> #include <Wire.h>
#include "esp_adc_cal.h" #include "esp_adc_cal.h"
#include <EEPROM.h>
// ID senzorja mora bit unikaten! (se poslje poleg parametrov) // ID senzorja mora bit unikaten! (se poslje poleg parametrov)
#define SENSOR_ID 5 #define SENSOR_ID 6
// Stanje baterije // Stanje baterije
#define BATTERY_PIN 2 #define BATTERY_PIN 2
@ -34,7 +35,9 @@
#include <WiFi.h> #include <WiFi.h>
// MAC naslov sprejemnika // MAC naslov sprejemnika
uint8_t sprejemnikMac[] = {0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C }; uint8_t sprejemnikMac[] = { 0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C };
// Comp
sensor_msg odcitek; sensor_msg odcitek;
esp_now_peer_info_t peerInfo; esp_now_peer_info_t peerInfo;
@ -47,6 +50,11 @@ imu::Vector<3> linearAccel;
int cas = 0; 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() { void error_blink() {
while(1) { while(1) {
digitalWrite(LED_PIN, LOW); digitalWrite(LED_PIN, LOW);
@ -75,20 +83,6 @@ void setup() {
delay(500); 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 init
WiFi.mode(WIFI_STA); WiFi.mode(WIFI_STA);
if (esp_now_init() != ESP_OK) { if (esp_now_init() != ESP_OK) {
@ -105,6 +99,77 @@ void setup() {
error_blink(); 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! // Running - led on!
digitalWrite(LED_PIN, HIGH); digitalWrite(LED_PIN, HIGH);

View File

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

View File

@ -1,12 +1,19 @@
// Dependencies: // Dependencies:
// - SLIPDecoder (https://git.kompot.si/g1smo/SLIPDecoder) // - SLIPDecoder (https://git.kompot.si/g1smo/SLIPDecoder)
// - MathLib (https://depts.washington.edu/dxscdoc/Help/Browse.html#Libraries%3EMathLib) // - 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 // Initialize the the receiver via SLIP decoder
~receiverPath = "/dev/ttyUSB0"; ~receiverPath = "/dev/ttyACM0";
~baudRate = 115200; ~baudRate = 230400;
OSCFunc.trace(true); // debug osc OSCFunc.trace(true); // debug osc
OSCFunc.trace(false); OSCFunc.trace(false);
@ -17,48 +24,76 @@ OSCFunc.trace(false);
// Indeksirani so z IDjem // Indeksirani so z IDjem
~senzorji = Dictionary.new; ~senzorji = Dictionary.new;
// Indeksirani so s parent IDjem
~starsi = Dictionary.new;
// Dodaj senzorje // 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)); ~senzorji.put(id, AHRSensor.new(id));
}); });
*/
// Olimex test // Olimex test
~senzorji.put(9, AHRSensor.new(9)); //~senzorji.put(9, AHRSensor.new(9));
//~senzorji.postln; //~senzorji.postln;
~decoder = SLIPDecoder.new(~receiverPath); ~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;}); ~elementi = ~senzorji.values.sort({ |a, b| a.id < b.id;}).collect({|s| s.getGui;});
~startStopDecoder = { |butt|
if ((~decoder.running.not), {
// If not running, start decoder
~decoder.trace(true); // debug slip decoder
~decoder = SLIPDecoder.new(~ttyInput.string, ~baudRate);
~decoder.start;
butt.string_("Stop")
}, {
// Else stop the decoder
~decoder.stop;
butt.string_("Start")
};
);
};
~ttyInput = TextField().string_(~receiverPath).keyDownAction_({ |input, char| if (char == $\r, { ~startStopDecoder.value(~ttyInput); }); });
~w.layout_( ~w.layout_(
VLayout( VLayout(
HLayout( HLayout(
// HEADER; serial path, buttons // HEADER; serial path, buttons
StaticText().string_("Serial path: "), StaticText().string_("Serial path: "),
~ttyInput, ~ttyInput,
Button().string_("Start").action_({ |butt| Button().string_("Start").action_(~startStopDecoder);
if ((~decoder.running.not), {
// If not running, start decoder
~decoder.trace(true); // debug slip decoder
~decoder = SLIPDecoder.new(~ttyInput.string, ~baudRate);
~decoder.start;
butt.string_("Stop")
}, {
// Else stop the decoder
~decoder.stop;
butt.string_("Start")
});
})
), ),
[],
// Sensor rows // Sensor rows
GridLayout.rows( GridLayout.rows(*~elementi.flatten)
*~elementi.flatten
)
) )
); );
@ -76,18 +111,27 @@ OSCFunc.trace(false);
// Quat listener // Quat listener
q = OSCdef.new((\quat ++ s.id), { |msg, time, addr, recvPort| q = OSCdef.new((\quat ++ s.id), { |msg, time, addr, recvPort|
var q; 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]); q = Quaternion.new(msg[4], msg[1], msg[2], msg[3]);
// Count quat events // Count quat events
Routine { Routine {
var quatD;
senzor.eps = senzor.eps + 1; senzor.eps = senzor.eps + 1;
senzor.quat = q; senzor.quat = q;
senzor.refreshGuiQuat; senzor.refreshGuiQuat;
senzor.updateEuler(q); //quatD = senzor.calibQuat.conjugate * q;
quatD = senzor.calibQuat.reciprocal * q;
senzor.updateEuler(quatD);
senzor.refreshGuiEuler; senzor.refreshGuiEuler;
senzor.euler.postln;
// Relativni euler koti (glede na starsa)
if (senzor.parent != nil) {
senzor.updateEulerD(q);
senzor.refreshGuiEulerD;
};
}.play(AppClock); }.play(AppClock);
}, oscHeader ++ "/quat"); }, oscHeader ++ "/quat");
@ -97,7 +141,17 @@ OSCFunc.trace(false);
var a; var a;
a = msg.at((1..3)); a = msg.at((1..3));
Routine { Routine {
var vectorSum = 0, xy = 0, xyz = 0;
senzor.accel = a; 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; senzor.refreshGuiAccel;
}.play(AppClock); }.play(AppClock);
}, oscHeader ++ "/acc"); }, oscHeader ++ "/acc");