diff --git a/AHRSensor/AHRSensor.sc b/AHRSensor/AHRSensor.sc index a0eaf7b..a728a6e 100644 --- a/AHRSensor/AHRSensor.sc +++ b/AHRSensor/AHRSensor.sc @@ -1,9 +1,15 @@ AHRSensor { var calibrationQuat, + calibQuat, + <>quat, <>euler, + <>eulerD, <>accel, + <>accelSum, <>battery, <>eps, @@ -14,30 +20,75 @@ AHRSensor { // Debug? -#define DEBUG +//#define DEBUG // Booleans #include @@ -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); diff --git a/linux-receiver/pozeni.sh b/linux-receiver/pozeni.sh index eceb19e..629c30e 100755 --- a/linux-receiver/pozeni.sh +++ b/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 diff --git a/platformio.ini b/platformio.ini index fbfa161..a4213c8 100644 --- a/platformio.ini +++ b/platformio.ini @@ -21,6 +21,10 @@ build_flags = -D ARDUINO_USB_CDC_ON_BOOT=1 build_src_filter = + +[env:calibration] +build_src_filter = + + + ;; Olimex prototype sketch [env:main-olimex] build_src_filter = diff --git a/src/calibration.cpp b/src/calibration.cpp new file mode 100644 index 0000000..e32b4f3 --- /dev/null +++ b/src/calibration.cpp @@ -0,0 +1,310 @@ +#include +#include +#include +#include +#include +#include +#include + +/* 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); +} diff --git a/src/main.cpp b/src/main.cpp index 32641b5..18b9500 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,9 +1,10 @@ #include #include #include "esp_adc_cal.h" +#include // ID senzorja mora bit unikaten! (se poslje poleg parametrov) -#define SENSOR_ID 5 +#define SENSOR_ID 6 // Stanje baterije #define BATTERY_PIN 2 @@ -34,7 +35,9 @@ #include // 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; 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); diff --git a/src/sprejemnik.cpp b/src/sprejemnik.cpp index e345df4..afa9063 100644 --- a/src/sprejemnik.cpp +++ b/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); - SLIPSerial.begin(115200); + //setCpuFrequencyMhz(80); + //SLIPSerial.begin(115200); + SLIPSerial.begin(230400); // Ne posiljaj preden se podatki napolnijo diff --git a/utopia.scd b/utopia.scd index c270472..2235ad5 100644 --- a/utopia.scd +++ b/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,48 +24,76 @@ 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;}); +~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_( 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 - ~decoder = SLIPDecoder.new(~ttyInput.string, ~baudRate); - ~decoder.start; - butt.string_("Stop") - }, { - // Else stop the decoder - ~decoder.stop; - butt.string_("Start") - }); - }) + Button().string_("Start").action_(~startStopDecoder); ), - [], // Sensor rows - GridLayout.rows( - *~elementi.flatten - ) + GridLayout.rows(*~elementi.flatten) ) ); @@ -76,20 +111,29 @@ 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"); // Acceleration listener @@ -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"); @@ -138,6 +192,6 @@ OSCFunc.freeAll; /********** * SOUND! * *********/ - + ~e = Env([1, 0.2, 0]); {[SinOsc.ar(50), SinOsc.ar(52.3)] * EnvGen.kr(~e, doneAction: Done.freeSelf)}.play; \ No newline at end of file