From 8ca3b235b788a58438bfbdce2353765436288f90 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= Date: Wed, 18 Sep 2024 13:56:49 +0200 Subject: [PATCH] Kalibracija, vecji baud rate sprejemnika, parent/child, druge posodobitve - vec gui-ja, euler pretvorbe, ... --- AHRSensor/AHRSensor.sc | 138 +++++++++++++--- linux-receiver/bin/receiver | Bin 17936 -> 17784 bytes linux-receiver/main.c | 23 ++- linux-receiver/pozeni.sh | 3 +- platformio.ini | 4 + src/calibration.cpp | 310 ++++++++++++++++++++++++++++++++++++ src/main.cpp | 97 +++++++++-- src/sprejemnik.cpp | 5 +- utopia.scd | 108 +++++++++---- 9 files changed, 606 insertions(+), 82 deletions(-) create mode 100644 src/calibration.cpp 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 { 5*DUT zBRI@tb;5?j1}qy}#VQqrD!>bPBQPkY(7v7f;ZR`Z zO1}4=kKgZ{d)~eGJ$Jq?rgw;)ZfCbcpe@7(;Ye-Q?BAHHCNZ(9>t8`ji6W^jrD zv){l(Kh%ps^N5j2L$Fo6+mhI9EFON#TxHvTy8<4<>)w)8pQee;vGn_;uj zUr>ndBL1={Y%A>x;iz$zXovGg7uXk@Am20x#wEA-0XR%9Xg0aUf0LP5#yn8|1Vkwa zX`@@Lqq^1TEz?=2G*8SXGjTh`yhD49_J5&$DfN3c%PsDvvOr|Xhe`S1pdlX)X8i<0 z*>j*hdm$Xp{#m3fus~S4tbWnrm&yX=^GU)6!H8=xPW1O?qF_P`v9X?6m@h$+*-DIL zCjz!#bnU`In1nT4w`uA+EWJ@!&*k^Jc1=eIgYX(ENK4eRF^3I8J6y|gc|uMm4u$Tw z4neUw&%>eAG*rhtNcoMh-0bp9W(wM${hHMcZDtp2Hd}Lr5Oc}K!2tWsF3|*kGGB$0 z7HjV6ui5Ny%Hk3oaKZ9h(F5^GF8ISFYwtDW_`C*0B|j zMpRCT(YBPdAf{|P#JH*{mxf%Lam(qL6q<+J z2~m|QanetSuvCROSJb3d{-v%fu>va*W717hm+HaRnwVrkjd~nCyR6jn2mxw0?&^9{ zx?D|)g)PI@PjJX~5zRdLm9|5|cFBc#Qbk-rg_IM3K!NMIc)=a=bj0Dl8Azh2rwLX= zEV`VbD@o90)*|h3lU+>8qa@l`9bP9fd6e3U(S|FwZ%f#=Jq~|%l|=H$QdIeDtYK7% z%LkNr?oqs&7(2`UJciIVlN?ofm=L`@tf<4vz=#oZ!NH)YgSg^_m_jVyfO!u$+Fl(b z$yFlgn*k02h}@6JdL&|W&O;&od56@lVOWVTKY*dn^0It|>U%5tujT#~*~yCDB}g^$ zs3mng9IufOe`5`vdKj?akpNrdVT9?Ch|ivUs0L@g_`fx{OVv^b0K9$>k<&rZD9cAZJ|@rLf0e7C<`#j}!Rd+e8} zSLJlHEnUtc?59KneV18;FYy9q5sW;-GDLhtGSr;6VOF6(a)eUch_>Be3<_ts&KUMg zFzltXq;@Gy%hrjxgpaTs|JFAZz5B@xj^+6wI;iq^v~8R*eolb{eK#57(-Vv@;Vg?3 z^o=z9qi|-4v6U35kGtPt?rFTw*%D?_LsDCs2H!M)M2U5bki8fS+3|#pHzqsAQOqZW zUF#>9jx;gH-ymw-<7xgR-^Y`y;w>4I#~JT?G(u8aMi!<@7YH~m{f=u_+|u>T&GX7^ z!kg4~qAsGu-xuhyONEbOszR7PlA~Un_GwsQ^t*z7s;uGNDE!EQoKeo1hmo#Ki8W|S zyn}GCEmna~;>td}uo2mUNXmEO7>10da+K&Fb8vdE^`7!Yvvzvz5cay^L+^^o<-+<6 zFRxkOwBZM>=mj&1myx$8nZ8>5#BBdnUEhwvo^f42fa6{qSK-)oP1lETeC?*Lr*L%N zg8t&!24x&B7YC$0|Izhkc+B^>I09jx>xltGT_U?DJlXzJ!{Xd5IchY1YP4qO2*NZ} zx8rZy4cOwFo@e@8*H@4ok#RLX*ZX^)W71A*XGuH%5XgKGWM2Kt_#`8*gt56rh9(bK z0xyb9ur5#}MnMhK;YZksK#{|bTa`4dDs4@08*P554i*_w*}X3WSBNk)&uthA;TO{s zOHR%8XRoeze>LAk#9nWyUvv!fuT0J$cp+Sd-*RO+6CW!XOFd3RVXDv#U1dSB9R|vl zmI+~I?dDF0sE&M9X)pAYFT^jyYvqM7V}8D81-@{+ zf=5V%7&_B$px=q@r1$K`(l{`Szw^i_()Jdy)*nF8jtt%0nqfGMdV*?S%rqu$_yF}z zgI#FyWIWk8KqIb)@B){32F}kfgi&L@m<25;7m(gF3v(rmLp&1SYC@1$>%-LQ5k^9c zFJpa%qnddh{UO>H5unC#}BR+1p#P!Wnn4%kNN!o47nL}z< zI^D=PAABCOQBOGSZib=f1gAlY3Za$Mc7)fqi8Cuy?-4GB7%KA=pCKsET8>I^6rM=h zzE7-=MTiw^^s4{E>mm+|tV3UgI4TWD2p$>OD=I0P+Ag62esDHE`7Nu@bc_Cb=(29oR|!31UVf`ov9UC1>zR(uDLQZYc%L@9)jRCJ7q zY`|RBw7@|{$C<896`k4+5R3#zSpsE4tr+c8@iETo(0VNqf<%ajZvXSRn_FV<&YttX zzVH0!zvrI&7~L-Hcv9Gr;T&`zdiYs~?pUyM=F67RWTuYp{J6kcl){vv6zmjymO~;_ z58-N|z=DiSQ}N@%WoMU{X=6R!E`FYRd+EuCCvAN1ug5;Tt@N8oUwk&}SYHo*)R32! z4dDX(Cs5eqw9UqShU?`)ofq96? z0Dl4gq$NKhF^#oGZXNS!5-kIG4X6W(V`nPbz4K`(lM!v_wu4kRpr=4jAwjK~jTSO+ z7}uIz(mp4{4OFn+%xRjqUG@L3UDG2AnVp;x5x-@2x%nBU=0Tm;fxj`k@Knphl$g$% zjk7E+H{_H!%zh>&L1(~AEa&0mR#;v5ChNqM2A$82e`DR7o56x<`-;x%*4g)AZ?Y@* zkj^_*=Z)y>Sv^I`lZ^-LyM&_50)!gUA@1vrpW^X&u9N64qBl|9N^~;%*Y{IB6Jy8p zdxoK;@wE45Av99&v*mPxA0b1+oRGDF`S(h zl7{3pgEScdSb#r5VW;%4xj)9^L1{>U;Q3PLqwgHC60#Ss#^X^*r93x=Bvsxccc)1- zi&Q(;Q}ByK}i*YZK~<9Q@jFqg^*%O16Wi#qj8;~Tqwas^%S#? zYbJ07r87Wv#_-swkZBtfEEo|KQyU0x6CRj*Eyyr;Re?<9P}*Ifw!;aYPt4{0E%QRA zJ3yrKRbjVGl<5{SGgBe>$f2Sfzwq*)(u!@duPPfc@Hj3%`bixB4tH79jb}R-BMWYPB{p77`E)#e(?}Q zs7@9(2t?!@WFF54(o_*v^kBVheLq>F?~cafn){jY?&rtc%O4o;eh}PpsAf>^UJ?e@ z(|qCYCa6Se=it_(Pwi#!ssVyjq(o}THOu^pNS_*TcL*H8AN9NhLp`R%w99^O+2;sJiM> z3fY5Wu;Illw%={TwdvJW*F%8qjfm3!wgJxe5%L(QUI$|<($!$w)p%&=joWmjM>f0p>?27!BA;K7nG#YFhWqf24Cw%7_ zU(`fY!uKwfqUwGiIzsYMANmfjbmf&~QHGIV+X!Pw;S84GiSKEAZ%q~mgr#z47t(>4`Kx0368Vrp_*a2 zdn+Mf>tioe_w1`c2oYbxsFd*JF(4#2ez>)pkHO>8d*UvSy|A)wQ;TOwVDUUpZej7} zdH=7mqQcF@=fO6Qd-d&W*EF`&J?N>f1X!aLZK_-U$cAPr)-DHAy~m1b@707AT-dlw z6SSC(TFgc*W_4`~4Z<@#%N@mNPFYcL5n8ulP3^jd4Vzl2B6 zZ4yI{RxW6I2mX3ij+mE)t-kw&eYnw=BfN$c-$G$IKIzMGge?q4vLQzde-FMeE-%Ot zSDJfP6f6~-PxDu}mwDgo!287+qUwh(-THj*;BmDttub~(o$ZoPb1os>da-*1^@n~(3zK`uDt@}?9?SOo~ z4KOdL^mv`d^wXI4!Dt7b2#?_*=rp|TO_@c|;l<1b7a=`{0pS$wC@+f}cz21izJP@? zmte-;IWECV+xfJ;g|zOKkTu~{`Dn05(PJ&~F8M9>!t2X5-cF9%!86g`^Axa?++%)D z!@H-!PODV(5=VAigM5|PMK@BfLH5!t2Pg{Gh=&nsGyjNshd5^g=d|)sFA}dHlKEG= z3?#Em^a_vRA?Wvjb;A15z9mfKj;Rn&?DI33Aq+JphmTgXg0w|)-S*xFD$TxxiyR({xVC9YcYJMzbvsKX&cjW{H(v-teshW z)bGM&0f!~R_o4{CGfT3z-QI2HQ4<=2;3|_A>hnKKCABA(Orv5EdyRw zuYsPOi>d@6hCg0hV2SyeF@_zLWftv>!C;jO@nEGH@2Q+?jTNyNPZIv5veb&onF{-0 Rrc`R3%|DA|!k`PX{|iThc_sh= diff --git a/linux-receiver/main.c b/linux-receiver/main.c index eb20528..cf54d14 100644 --- a/linux-receiver/main.c +++ b/linux-receiver/main.c @@ -35,7 +35,7 @@ $ sudo ifconfig wlp5s0 up #include // 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