Diff: main.cpp

Revision:

2:0cf2ef636323

Parent:

1:4c27d179c6c2

--- a/main.cpp	Mon Aug 26 09:47:32 2019 +0000
+++ b/main.cpp	Sat Sep 07 16:55:08 2019 +0000
@@ -1,8 +1,7 @@
 #include "mbed.h"
-//#include <math.h> //Probo sma bez toga radi sve pa nije ni potrebno mozda tj tamo kad ubacis onaj library i kod za akcelometar ce trebat mozda vidi sta taj main ima includano
-//#include "SSD1308.h" //lib za ekran to cu ti isto donjet jedan dan
-
-//#define AVG 5 //Broj mjerenja za pocetni gps, ne koristi se jer nema gpsa
+#include "MPU9250.h"
+#include <math.h> 
+#include "SSD1308.h" 
 #define cal_num 10 // broj ocitanja analognih ulaza za kalibraciju, za dobivanje srednje vrijednosti
 
 //Analogni ulazi 
@@ -14,7 +13,6 @@
 AnalogIn a6(PA_6);
 AnalogIn abat(PA_7);
 
-//Digitalni ulazi SW su oni gumbici desno (3) a JSW su gumbici kad stisnes joystick
 DigitalIn SW1(PA_11);
 DigitalIn SW2(PB_5);
 DigitalIn SW3(PB_4);
@@ -24,77 +22,39 @@
 DigitalIn TSW(PB_1);
 //Serijska veza sa modulom za radio komunikaciju to samo prosljeduje poruku koda koristi print na PC i tera term
 Serial hc(PA_9, PA_10);
-//Serial pc(PA_2, PA_15);
+Serial pc(PA_2, PA_15);
 
-//Proglasio i2c pinove za prosljedivanje ekranu al mozda ces moc i prosljedit akcelometru odmah
-I2C i2c(PB_7, PB_6);
-//Tak je pisalo u biblioteci da se proglasava
+//I2C i2c(PB_7, PB_6);
+
 //SSD1308 oled = SSD1308(&i2c, SSD1308_SA0);
 
-// GPS //
-/*
-char data[300];
-int i=0;
-char ndone=1;
-char avg_count=0;
-
-float LAT,LON,TIME;
-char NS,EW,status;
-float check;
-int latdeg,londeg;
-float latmin,lonmin;
-float lat_arr[AVG];
-float lon_arr[AVG];
-*/
-
-short TH,PI,RO,YA,TH_def;//Throttle pitch roll yaw i TH_def, th def mos promjenit ime u nes smisleno tipa th_novi il tak nes jer ovo def nemam pojma zas sam stavio nema nikakve veze s nicim
+short TH,PT,RO,YA,TH_def;//Throttle pitch roll yaw i TH_def
 float x1_ar,x2_ar,x3_ar,y1_ar,y2_ar,y3_ar; //privremene varijable za zbrajanje ulaza, ocitava se znaci analagni ulaz i zbroji sa prijasnjim pa se na krjau sve podjeli s broje ocitanja 
-//realno bolje je da ovu liniju stavis u funkciju dolje od kalibracije posto se ti brojevi samo koriste u funkciji al nije bitno 
-short mid_point=50; //to samo drzi vrijednos tkoju smatramo sredinom u nasem slucaju 50 (analogni ulaz ide od 0 do 1.0 pomnozen sa sto od 0 do 100%)
-short x1_cor,x2_cor,x3_cor,y1_cor,y2_cor,y3_cor; //ovo drzi korekcijski broj
+short mid_point=50; //vrijednost koju smatramo sredinom u nasem slucaju 50 (analogni ulaz ide od 0 do 1.0 pomnozen sa sto od 0 do 100%)
+short x1_cor,x2_cor,x3_cor,y1_cor,y2_cor,y3_cor; // korekcijski broj
 short low_us=1350; //inace 1000 al da smanjimo osjetljivost joysticka smo stavili 1350 pa ide do 1650
 short multiplier_us=3; // s tim mnozimo nas izracunati analogni ulaz u nominalnom stanju 50*3 znaci 150 + 1350 =1500us posto dron prima od 1000 do 2000us s tim da
-// za ove vrijednosti roll pitch yaw je 1500 nominalno di se ne dogada nista a pomak prema 2000 il 1000 oznacava zakret oko osi to si mos ili u mom radu pogled ili guglas pitch yaw roll i pogleda slike da znas sta je oko koje osi
-//ako je pozitivni smjer x osi u smjeru gibanja drona onda ti je yaw zakret oko Z osi  znaci dron se okrece ljevo desno, pitch je pomak oko Y dron ima nagib naprijed nazad i roll je oko x osi znaci dron ima nagib ljevo il desno
-
-//flag za gps kad je 0 hocemo upravljat mi dronom ako se digne u 2 gps upravlja dronom
-int rcom=0;
-//nemam pojma sta sma tu mislio ne  koristi se za nis
-//int arcom;
+// za ove vrijednosti roll pitch yaw je 1500 nominalno di se ne dogada nista a pomak prema 2000 il 1000 oznacava zakret oko osi 
 
-//gps funkcije
-/*
-void reads();
-void rounder();
-int get_deg(float);
-float get_min(float);
-void avg_read();
-*/
+int rcom=0;
+
 //spremanje napona baterije
 float vbat;
-//vrijednost otpornickog djelila znaci ulazni napon  podjeljeno s tim daje napon baterije s tim da ulazni napon tj ocitanje analognog ulaza (0-1) moras prvo pomnoziti sa 3.3 
+//vrijednost otpornickog djelila znaci ulazni napon  podjeljeno s tim daje napon baterije s tim da ulazni napon tj ocitanje analognog ulaza (0-1)* 3.3 
 float bat_divider=0.703313;
-
-//moras prvo proglasiti koliko je dugacak string za oled (,malo cudno), a posto je string ustvari array char to radis tako, ovjde mozemo imat 30 znakova u nasoj poruci z aoled moze s epovecat
 char oled_str[30];
-
-//prototip funkcije da ju mos pozvat u main makar je napravljena ispod maina
-//kalibracija analognih ulaza
 void calibration();
-//skaliranje throttlea da ne ide od 1000 do 2000 nego da se postepeno penje za 10 ovisno jel drzis joystick prema gore ili dolje
 void th_calc();
 
 int main(){
-    //switchevi su spojeni s jedne strane na mbed s druge na 3.3v pa ih pull downam da budu default 0 pa kad ih spojim skoce na 1
+    
     SW1.mode(PullDown);
     SW2.mode(PullDown);
     SW3.mode(PullDown);
     JSW1.mode(PullDown);
-    //naravno kinezi sposobni imaju svoj krug na joysticku za gumb i na jednom gumbu je isto tak pin spojen na mbed a drugi na VCC  il sta vec pise na toj maloj plocici od joysticka
-    //ali na druga 2 joysticka je spojen na pin mbeda i na gnd pa sam moro default stavit pull up tj 1 a onda kad stisnes gumb id eu 0
     JSW2.mode(PullUp);
-    JSW3.mode(PullUp);//default 1 uvjet ako 0
-    //Onaj switch izmedu 2 joysticka
+    JSW3.mode(PullUp);
+    //switch izmedu 2 joysticka
     TSW.mode(PullDown);
     //kalibraciju vrsim nakon sto ih sve pull up/downam
     calibration();
@@ -107,12 +67,7 @@
         //sprintf(oled_str, "%d", TH_def);
         //oled.writeString(0, 0, oled_str);
         
-        //ok znaci to sma ti prico vrti se petlja i ako je taj switch na 1 onda se gledaju analogni ulazi i radi joystick a ako je na 0 salje se default dronu tkao da s ene dogada nista i ne cita inpute korisnika
         if(TSW.read()){
-            //gps opet
-            //reads();
-            //avg_read();
-            //rounder();
             //ulaz znaci npr 0.54 default *100 =54- korekcija izracunata u kalibraciji pa je blizu 50 nekad 49 nekad 51 ali blize nego bez korekcije, *5=250
             //-250=0 za default nema spustanja il dizanja TH, recimo da dizes sto je u nasem slucaju zbog naopacke zalemljenog joysticka ustvari prema dolje (zato je -1 tu)
             //ond aje npr 0.74*100 -korekcija =70*5=350 -250=100 pozitivan broj ali *-1 posto smo ustvari joystick pomakli prema dolje i logicno je da je to spustanje gasa
@@ -122,44 +77,43 @@
             // slicno kao kod th samo ovdje nam dalje pretvorbi nego se direktno vrijednost salje dronu
             //ocitano npr 0.53*100 - kalibracija =50 * multi koji je 3 znaci 150 + low us 1350 je 1500 i zbog obrnutih joysticka ovja put 3000-1500=1500
             //ali da je broj npr 1250, 3000-1250= 1750 (recimo za pitch unaprijed dron treba vise od 1500 ali mi kad taj joystick pomaknemo naprijed dobijemo manje od 1500 pa zato 3000-X nam daje suprotnu vrijednost
-            RO =3000-(low_us+((a3.read()*100)+y1_cor)*multiplier_us); 
+            PT =3000-(low_us+((a3.read()*100)+y1_cor)*multiplier_us); 
             YA =3000-(low_us+((a2.read()*100)+x2_cor)*multiplier_us);
-            PI =3000-(low_us+((a4.read()*100)+y2_cor)*multiplier_us);
+            RO =3000-(low_us+((a4.read()*100)+y2_cor)*multiplier_us);
+            double getme ();
+            
             //jos uvijek nez za sta sam to koristio
-            //arcom=a5.read()*100;
-        //svakim prolazom kroz petlju vracam rcom na 0 jer je to default i tek kad stisnes on postane nes drugo
+        //svakim prolazom kroz petlju vracam rcom na 0 jer je to default i tek kad stisnes on postane nes drugo, flag za gps i slično
             rcom=0;
-            //switch desno u sredini sluzi za gps makar je sad na dronu iskljucen pa ga mos stiskat kolko hoces 
+            //switch desno u sredini sluzi za gps
             if(SW2.read()==1){
                 rcom=2;
             }
-            //switch skroz dolje desno sluzi za  Arming drona moras mu poslat tocno te vrijednosti 
+            //switch skroz dolje desno sluzi za Arming drona 
             if(SW3.read()==1){
                 hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",1000,1000,1000,1000,2000,0,0,0);
             }
-            //ako nisi nista stisnuo salje se ono sa inputa normalno inace ak ne diras nes th_def je 1000 pi ro ya su 1500 rcom 0 a ove 2 nule izmedu su za longitude i latitude od gpsa
+            
             else{
-                hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,PI,RO,YA,1000,0,0,rcom);
+                hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,RO,PT,YA,1000,0,0,rcom);
+                pc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,RO,PT,YA,1000,0,0,rcom);
+                pc.printf("%0.2f\n",vbat);
+                pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
             }
             //testiranje na pcu
             //printf("%hd,%hd,%hd,%hd,%hd,%hd\n",x1,y1,x2,y2,sw1.read(),sw2.read());
             //printf("%hd,%hd,%hd,%hd\n",sw1.read(),sw2.read(),sw3.read(),tog1.read());
             
-            //napon baterije mos pokazat na ekranu kao, bilo bi jebeno da sam implementiro da dron salje napon baterije svoje al jbg to nije tvoj dio ti mos na ekranu napisat Dron bat=? kao da si napravio sve d amoze primit al huda nije slao
-            //vbat=(abat.read()*3.3f)/bat_divider;
+            vbat=(abat.read()*3.3f)/bat_divider;
             //ispis na komp
-            //printf("%0.2f\n",vbat);
-            //zelimo da salje sto vise ali opet ne zelimo pretjerati 10ms je ok , dron nece pola tih poruka procitat jer nema prekida na dronu za serijski ulaz pa ustvari svakim prolaz kroz
-            // glavnu petlju drona ja gledam jel ima sta, ovak brzim slanjem izbjegavamo to da se mimoidu pa da dron mora bit u petlji cekat poruku jer daljinski salje tolko brzo da dron nikad ne mora cekat ustvari
+            pc.printf("%0.2f\n",vbat);
             wait_ms(10);//
         
         }
-        //ako switch nije ukljucen salje se default poruka da sse ne doagada nista
+        
         else{
             
-            
             hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",1000,1500,1500,1500,2000,0,0,0);
-            //to ne moramo slat tako cesto jer se ne dogada nis dron je na tlu nema letenja
             wait_ms(100);
         
         }
@@ -167,14 +121,14 @@
 }
 
 void calibration(){
-    //postavljane tih varijabli u 0
+    
     x1_ar=0;
     x2_ar=0;
     x3_ar=0;
     y1_ar=0;
     y2_ar=0;
     y3_ar=0;
-    //citanje ulaza zbrajanje sa prijasnjim
+    
     for(int a=0;a<cal_num;a++){
         x1_ar+=a1.read();
         x2_ar+=a2.read();
@@ -183,10 +137,6 @@
         y2_ar+=a4.read();
         y3_ar+=a6.read();       
     }
-    //uzimamo sve zbojrene npr postose kalibracija radi dok ne mices nista recimo  je x1_ar =5.4
-    // /10 mjerenja je srednja vrijednost 0.54*100=54 midpoint ili ti 50-54 je -4
-    //korekcijskia vrijednost je -4 znaci da kad se gore mjeri ulaz koji mozda nece bit svaki put 54 jer je to srednja vrijednost ali kakav god da bio zbrojit ce se korekcija sa njim u ovom slucaju negativna 
-    // pa ce to dat 50 lijepo il 49 il 51 mozda nekad i gore vrijednost ali u prosjeku ce bit blize 50 nego bez kalibracije
     x1_cor=mid_point-((x1_ar/cal_num)*100);
     x2_cor=mid_point-((x2_ar/cal_num)*100);
     x3_cor=mid_point-((x3_ar/cal_num)*100);
@@ -197,14 +147,12 @@
 }
 
 void th_calc(){
-    //ovo prvo sluzi tome da ne registriramo smetnje dok ne diramo throttle
-    //jer moze vrijednost skakat malo  pa sam ovako odredio da nisu te greske vece od 10, mos stavit i 20 radi sigurnosti jer i tako joystick sada mos pomaknut skroz i bit ce isto ko kad ga pomaknes malo 
-    //jer se ne gleda sama vrijednost nego jel pozitivna il negativna (osim naravno u toj prvoj provjeri za smetnje)
-    if(abs(TH)<10){
+    
+    if(abs(TH)<20){
         TH_def+=0;
     }
     else{
-        //ako je veca povecaj th def za 10 ak je manja od 0 smanji th def za 10
+        //ako je veca povecaj th def za 10, ako je manja od 0 smanji th def za 10
         if(TH>0){
             TH_def+=10;
         }
@@ -215,76 +163,178 @@
     //sprjecavamo da ode izvan granica
     if(TH_def<1000)TH_def=1000;
     if(TH_def>2000)TH_def=2000;
-    // u slucaju da osoba stisne gumb na joysticku ljevo gore th se spusta na 1000 th zaustavlaju se motori
-    //sad bjaza moze pitat zas to nije prekid na sto nemam dobar odgovor jer se ne sjecam jel bilo problema kad sam stavio prekid il jednostavno nije bio potreban
-    //ali mozes argumentirat da ako stisnemo gum u trenutku slanja poruke prekid moze zasmetat i mozemo poslat "losu" poruku dali ce to izazvat probleme vjerojatno ne jer ce se 10ms kasnije poslat druga poruka 
-    // a i realno slanje poruka unutar prekida nije najbolje rjesenje jer se izbjegavaju radit takve stvari u prekidu  pa kad bi u prekid samo stavili ovo th_def=1000 opet bi se to tek poslalo kad bi se slala slijedeca poruka 
-    //petlja se odvija toliko brzo da prekid stvarno nije potreban pogotovo kad uzmemo brzinu ljudskih refleksi u obzir tih par ms je nebitno, mislim onaj delay od 10 se moze maknut i petlja se ce odvijat maksimalnom brzinom
-    //bez negativnih posljedica
-    if(JSW1.read()==1)TH_def=1000;
+    // u slucaju da osoba stisne gumb na joysticku lijevo th se spusta na 1000 th zaustavlaju se motori
+    if(JSW2.read()==1)TH_def=1000;
 }
-//// GPS /////
-/*
+
+//Spojio na standardne pinove 14,15  s desne strane ploce mislimd a su to pb9 i pb8
+//onda ko i kog magnetometras idu otpornici sa sda i scl na 3.3v
+//VCC 5 ili 3.3 probaj s 3.3 ak
+//GND gnd
+//SCL scl
+//sda sda
+//eda i ecl na nista
+//AD0 na GND
+//i ova ostala 3 na nista, INT,NCS,FSYNC
+
 
-void reads(){
-    while(avg_count<AVG){
-        while(gps.getc() != '$');
-        while(ndone){
-            data[i]=gps.getc();
-            pc.printf("%c",data[i]);
-            if(data[i]=='\r'){
-                data[i]='\0';
-                ndone=0;
-            }
-            i++;
-        }
-        //GPGLL,4540.45939,N,01539.27211,E,164202.00,A,A*64
-        if(sscanf(data,"GPGLL,%f,%c,%f,%c,%f,%c,%f",&LAT, &NS, &LON, &EW, &TIME, &status, &check) >= 1) {
-            pc.printf("%s\n",data);
-            lat_arr[avg_count]=LAT;
-            lon_arr[avg_count]=LON;
-            avg_count++;
-            
-                    time = time + 20000.00f;
-                    hour = int(time) / 10000;
-                    minute = (int(time) % 10000) / 100;
-                    seconed = int(time) % 100;
-             
-             
-        }
-        memset(data, 0, i);
-        ndone=1;
-        i=0;
-    }
+float sum = 0;
+uint32_t sumCount = 0;
+
+MPU9250 mpu9250;
+
+Timer t;
+
+//Serial pc(PA_2, PA_3); // tx, rx
+
+volatile bool newData = false;
+
+InterruptIn isrPin(D12);   //k64 D12  dragon PD_0
+
+void mpuisr()
+{
+    newData=true;
 }
 
-void rounder(){ 
-     latdeg=get_deg(LAT);
-     londeg=get_deg(LON);
-     latmin=get_min(LAT);
-     lonmin=get_min(LON);
-     pc.printf("%d %.3f, %d %.3f\n",latdeg,latmin,londeg,lonmin);
-}
+void ziroskop()
+{
+    pc.baud(9600);
+
+    //Set up I2C
+    i2c.frequency(400000);  // use fast (400 kHz) I2C
+
+    pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
+
+    t.start();
+    isrPin.rise(&mpuisr);
+
+    // Read the WHO_AM_I register, this is a good test of communication
+    uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
+    pc.printf("I AM 0x%x\n\r", whoami);
+    pc.printf("I SHOULD BE 0x71\n\r");
+
+    if (whoami == 0x71) { // WHO_AM_I should always be 0x68
+        pc.printf("MPU9250 is online...\n\r");
+        wait(1);
+
+
+        mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
+        mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+      
+        wait(2);
+        mpu9250.initMPU9250();
+        mpu9250.initAK8963(magCalibration);
+        if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
+        if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
+        if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
+        if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
+        wait(2);
+    } else {
+        pc.printf("Could not connect to MPU9250: \n\r");
+        pc.printf("%#x \n",  whoami);
+
+
+        while(1) ; // Loop forever if communication doesn't happen
+    }
+
+    mpu9250.getAres(); // Get accelerometer sensitivity
+    mpu9250.getGres(); // Get gyro sensitivity
+    mpu9250.getMres(); // Get magnetometer sensitivity
+    pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
+    pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
+    pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
+    magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
+    magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
+    magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
+
+    while(1) {
+        static int readycnt=0;
+        // If intPin goes high, all data registers have new data
+
+#if USE_ISR
+        if(newData) {
+            newData=false;
+            mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS);  //? need this with ISR
+#else
+        if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
+#endif
+            readycnt++;
+            mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values
+            // Now we'll calculate the accleration value into actual g's
+            ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
+            ay = (float)accelCount[1]*aRes - accelBias[1];
+            az = (float)accelCount[2]*aRes - accelBias[2];
 
-int get_deg(float fnum){
-    return floor(fnum/100.0f);
+            mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
+            // Calculate the gyro value into actual degrees per second
+            gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
+            gy = (float)gyroCount[1]*gRes - gyroBias[1];
+            gz = (float)gyroCount[2]*gRes - gyroBias[2];
+
+            mpu9250.readMagData(magCount);  // Read the x/y/z adc values
+            // Calculate the magnetometer values in milliGauss
+            // Include factory calibration per data sheet and user environmental corrections
+            mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
+            my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
+            mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
+        }
+
+        Now = t.read_us();
+        deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+        lastUpdate = Now;
+
+        sum += deltat;
+        sumCount++;
+
+
+        // Pass gyro rate as rad/s
+        uint32_t us = t.read_us();
+        mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
+        us = t.read_us()-us;
+        int delt_t = 0; // used to control display output rate
+        int count = 0;  // used to control display output rate
+        // Serial print and/or display at 0.5 s rate independent of data rates
+        delt_t = t.read_ms() - count;
+        if (delt_t > 500) { // update LCD once per half-second independent of read rate
+            pc.printf("readycnt %d us %d\n",readycnt,us);
+            readycnt=0;
+            pc.printf("ax = %f", 1000*ax);
+            pc.printf(" ay = %f", 1000*ay);
+            pc.printf(" az = %f  mg\n\r", 1000*az);
+
+            pc.printf("gx = %f", gx);
+            pc.printf(" gy = %f", gy);
+            pc.printf(" gz = %f  deg/s\n\r", gz);
+
+            pc.printf("gx = %f", mx);
+            pc.printf(" gy = %f", my);
+            pc.printf(" gz = %f  mG\n\r", mz);
+
+            tempCount = mpu9250.readTempData();  // Read the adc values
+            temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
+            pc.printf("temperature = %f  C\n\r", temperature);
+
+            pc.printf("q0 = %f\n\r", q[0]);
+            pc.printf("q1 = %f\n\r", q[1]);
+            pc.printf("q2 = %f\n\r", q[2]);
+            pc.printf("q3 = %f\n\r", q[3]);
+            
+            yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+            pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+            roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+            pitch *= 180.0f / PI;
+            yaw   *= 180.0f / PI;
+            yaw   += 3.95f; // Godine 2018 magnetna deklinacija je +3* 57’ za Zagreb
+            roll  *= 180.0f / PI;
+
+            pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
+           
+            count = t.read_ms();
+            sum = 0;
+            sumCount = 0;
+        
+        }
+    }
+
 }
 
-float get_min(float fnum){
-    unsigned int tempmini=(int(fnum*1000))%100000;
-    return tempmini/1000.0f;
-}
-
-void avg_read(){
-    LAT=0;
-    LON=0;
-    for(int k=0;k<AVG;k++){
-        LAT+=lat_arr[k];
-        LON+=lon_arr[k];
-    }
-    LAT=LAT/AVG;
-    LON=LON/AVG;
-}
-
-*/
-