Kalibracija, vecji baud rate sprejemnika, parent/child, druge
posodobitve - vec gui-ja, euler pretvorbe, ...main
parent
b51daa1b00
commit
8ca3b235b7
|
|
@ -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|
|
||||
^super.newCopyArgs(id).init;
|
||||
*new { |id, parent = nil|
|
||||
^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,
|
||||
yy = y * y2, yz = y * z2, zz = z * z2,
|
||||
wx = w * x2, wy = w * y2, wz = w * 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
|
||||
e[1] = asin( (2 * q[1] * q[3]) + (2 * q[0] * q[2])).neg; // theta
|
||||
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];
|
||||
var m21 = matrix[1], m22 = matrix[5], m23 = matrix[9];
|
||||
var m31 = matrix[2], m32 = matrix[6], m33 = matrix[10];
|
||||
|
||||
var e = [0, 0, 0];
|
||||
m11 = matrix[0]; m12 = matrix[4]; m13 = matrix[8];
|
||||
m21 = matrix[1]; m22 = matrix[5]; m23 = matrix[9];
|
||||
m31 = matrix[2]; m32 = matrix[6]; m33 = matrix[10];
|
||||
|
||||
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),
|
||||
nil,
|
||||
StaticText().font_(Font("OpenSans", 12, true)).string_(napis),
|
||||
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 = euler + this.quat2euler(quatDiff);
|
||||
euler = this.quat2euler(newQuat);
|
||||
}
|
||||
|
||||
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 {
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -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);
|
||||
printf("%s\n", sporocilo);
|
||||
lo_bundle_free(svezenj);
|
||||
#endif
|
||||
|
||||
//printf("%s\n", glava);
|
||||
//free(sporocilo);
|
||||
lo_send_bundle(osc_dest, svezenj);
|
||||
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", "57121");
|
||||
osc_dest = lo_address_new("localhost", scport);
|
||||
|
||||
printf("\n Waiting to receive packets ........ \n");
|
||||
gettimeofday(&cas, NULL);
|
||||
|
|
|
|||
|
|
@ -1,5 +1,6 @@
|
|||
#!/bin/bash
|
||||
|
||||
dev=${1:-wlp3s0}
|
||||
oscport=${1:-57120}
|
||||
|
||||
sudo ./bin/receiver $dev
|
||||
sudo ./bin/receiver $dev $oscport
|
||||
|
|
|
|||
|
|
@ -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 =
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
95
src/main.cpp
95
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);
|
||||
|
||||
|
|
|
|||
|
|
@ -65,8 +65,9 @@ void prejemPodatkov(const uint8_t * mac_addr, const uint8_t * noviPodatki, int l
|
|||
|
||||
void setup() {
|
||||
// Nizja CPU frekvenca
|
||||
setCpuFrequencyMhz(80);
|
||||
SLIPSerial.begin(115200);
|
||||
//setCpuFrequencyMhz(80);
|
||||
//SLIPSerial.begin(115200);
|
||||
SLIPSerial.begin(230400);
|
||||
|
||||
|
||||
// Ne posiljaj preden se podatki napolnijo
|
||||
|
|
|
|||
96
utopia.scd
96
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";
|
||||
~baudRate = 115200;
|
||||
~receiverPath = "/dev/ttyACM0";
|
||||
~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);
|
||||
~ttyInput = TextField().string_(~receiverPath);
|
||||
~w = Window.new("Utopia || C²", Rect(300, ~senzorji.size * 60, 600, ~senzorji.size * 100),true);
|
||||
|
||||
// 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_(
|
||||
VLayout(
|
||||
HLayout(
|
||||
// HEADER; serial path, buttons
|
||||
StaticText().string_("Serial path: "),
|
||||
~ttyInput,
|
||||
Button().string_("Start").action_({ |butt|
|
||||
~startStopDecoder = { |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(
|
||||
*~elementi.flatten
|
||||
)
|
||||
GridLayout.rows(*~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");
|
||||
|
|
|
|||
Loading…
Reference in New Issue