Team H - TVZ Seminar / Mbed 2 deprecated Zavrsni_Daljinski

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed SSD1308_128x64_I2C

main.cpp@2:0cf2ef636323, 2019-09-07 (annotated)

Committer:
jjokocha
Date:
Sat Sep 07 16:55:08 2019 +0000
Revision:
2:0cf2ef636323
Parent:
1:4c27d179c6c2 
kommit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DudeHD 1:4c27d179c6c2 1 #include "mbed.h"
jjokocha 2:0cf2ef636323 2 #include "MPU9250.h"
jjokocha 2:0cf2ef636323 3 #include <math.h>
jjokocha 2:0cf2ef636323 4 #include "SSD1308.h"
DudeHD 1:4c27d179c6c2 5 #define cal_num 10 // broj ocitanja analognih ulaza za kalibraciju, za dobivanje srednje vrijednosti
DudeHD 0:ac8a5576c57a 6
DudeHD 1:4c27d179c6c2 7 //Analogni ulazi
DudeHD 0:ac8a5576c57a 8 AnalogIn a1(PA_0);
DudeHD 0:ac8a5576c57a 9 AnalogIn a2(PA_1);
DudeHD 0:ac8a5576c57a 10 AnalogIn a3(PA_3);
DudeHD 0:ac8a5576c57a 11 AnalogIn a4(PA_4);
DudeHD 0:ac8a5576c57a 12 AnalogIn a5(PA_5);
DudeHD 0:ac8a5576c57a 13 AnalogIn a6(PA_6);
DudeHD 0:ac8a5576c57a 14 AnalogIn abat(PA_7);
DudeHD 0:ac8a5576c57a 15
DudeHD 0:ac8a5576c57a 16 DigitalIn SW1(PA_11);
DudeHD 0:ac8a5576c57a 17 DigitalIn SW2(PB_5);
DudeHD 0:ac8a5576c57a 18 DigitalIn SW3(PB_4);
DudeHD 0:ac8a5576c57a 19 DigitalIn JSW1(PA_12);
DudeHD 0:ac8a5576c57a 20 DigitalIn JSW2(PB_0);
DudeHD 0:ac8a5576c57a 21 DigitalIn JSW3(PA_8);
DudeHD 0:ac8a5576c57a 22 DigitalIn TSW(PB_1);
DudeHD 1:4c27d179c6c2 23 //Serijska veza sa modulom za radio komunikaciju to samo prosljeduje poruku koda koristi print na PC i tera term
DudeHD 0:ac8a5576c57a 24 Serial hc(PA_9, PA_10);
jjokocha 2:0cf2ef636323 25 Serial pc(PA_2, PA_15);
DudeHD 1:4c27d179c6c2 26
jjokocha 2:0cf2ef636323 27 //I2C i2c(PB_7, PB_6);
jjokocha 2:0cf2ef636323 28
DudeHD 0:ac8a5576c57a 29 //SSD1308 oled = SSD1308(&i2c, SSD1308_SA0);
DudeHD 0:ac8a5576c57a 30
jjokocha 2:0cf2ef636323 31 short TH,PT,RO,YA,TH_def;//Throttle pitch roll yaw i TH_def
DudeHD 1:4c27d179c6c2 32 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
jjokocha 2:0cf2ef636323 33 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%)
jjokocha 2:0cf2ef636323 34 short x1_cor,x2_cor,x3_cor,y1_cor,y2_cor,y3_cor; // korekcijski broj
DudeHD 1:4c27d179c6c2 35 short low_us=1350; //inace 1000 al da smanjimo osjetljivost joysticka smo stavili 1350 pa ide do 1650
DudeHD 1:4c27d179c6c2 36 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
jjokocha 2:0cf2ef636323 37 // za ove vrijednosti roll pitch yaw je 1500 nominalno di se ne dogada nista a pomak prema 2000 il 1000 oznacava zakret oko osi
DudeHD 0:ac8a5576c57a 38
jjokocha 2:0cf2ef636323 39 int rcom=0;
jjokocha 2:0cf2ef636323 40
DudeHD 1:4c27d179c6c2 41 //spremanje napona baterije
DudeHD 0:ac8a5576c57a 42 float vbat;
jjokocha 2:0cf2ef636323 43 //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
DudeHD 0:ac8a5576c57a 44 float bat_divider=0.703313;
DudeHD 0:ac8a5576c57a 45 char oled_str[30];
DudeHD 0:ac8a5576c57a 46 void calibration();
DudeHD 0:ac8a5576c57a 47 void th_calc();
DudeHD 0:ac8a5576c57a 48
DudeHD 0:ac8a5576c57a 49 int main(){
jjokocha 2:0cf2ef636323 50
DudeHD 0:ac8a5576c57a 51 SW1.mode(PullDown);
DudeHD 0:ac8a5576c57a 52 SW2.mode(PullDown);
DudeHD 0:ac8a5576c57a 53 SW3.mode(PullDown);
DudeHD 0:ac8a5576c57a 54 JSW1.mode(PullDown);
DudeHD 0:ac8a5576c57a 55 JSW2.mode(PullUp);
jjokocha 2:0cf2ef636323 56 JSW3.mode(PullUp);
jjokocha 2:0cf2ef636323 57 //switch izmedu 2 joysticka
DudeHD 0:ac8a5576c57a 58 TSW.mode(PullDown);
DudeHD 1:4c27d179c6c2 59 //kalibraciju vrsim nakon sto ih sve pull up/downam
DudeHD 0:ac8a5576c57a 60 calibration();
DudeHD 1:4c27d179c6c2 61 //funkcije za display
DudeHD 0:ac8a5576c57a 62 //oled.setDisplayFlip(false,false);
DudeHD 0:ac8a5576c57a 63 //oled.setContrastControl(0xFF);
DudeHD 0:ac8a5576c57a 64 while(1){
DudeHD 1:4c27d179c6c2 65 //display sve
DudeHD 0:ac8a5576c57a 66 //oled.clearDisplay();
DudeHD 1:4c27d179c6c2 67 //sprintf(oled_str, "%d", TH_def);
DudeHD 0:ac8a5576c57a 68 //oled.writeString(0, 0, oled_str);
DudeHD 1:4c27d179c6c2 69
DudeHD 0:ac8a5576c57a 70 if(TSW.read()){
DudeHD 1:4c27d179c6c2 71 //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
DudeHD 1:4c27d179c6c2 72 //-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)
DudeHD 1:4c27d179c6c2 73 //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
DudeHD 1:4c27d179c6c2 74 // pa u funkciji tj_calc vidimo d aje vrijednost negativna i dok god je negativna svakim prolazom kroz ovu petlju spustamo za 10, da je joystick prema gore bila bi pozitivna i dizali bi z a10
DudeHD 0:ac8a5576c57a 75 TH =(-1)*((((a1.read()*100)+x1_cor)*5)-250);
DudeHD 0:ac8a5576c57a 76 th_calc();
DudeHD 1:4c27d179c6c2 77 // slicno kao kod th samo ovdje nam dalje pretvorbi nego se direktno vrijednost salje dronu
DudeHD 1:4c27d179c6c2 78 //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
DudeHD 1:4c27d179c6c2 79 //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
jjokocha 2:0cf2ef636323 80 PT =3000-(low_us+((a3.read()*100)+y1_cor)*multiplier_us);
DudeHD 0:ac8a5576c57a 81 YA =3000-(low_us+((a2.read()*100)+x2_cor)*multiplier_us);
jjokocha 2:0cf2ef636323 82 RO =3000-(low_us+((a4.read()*100)+y2_cor)*multiplier_us);
jjokocha 2:0cf2ef636323 83 double getme ();
jjokocha 2:0cf2ef636323 84
DudeHD 1:4c27d179c6c2 85 //jos uvijek nez za sta sam to koristio
jjokocha 2:0cf2ef636323 86 //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
DudeHD 0:ac8a5576c57a 87 rcom=0;
jjokocha 2:0cf2ef636323 88 //switch desno u sredini sluzi za gps
DudeHD 0:ac8a5576c57a 89 if(SW2.read()==1){
DudeHD 0:ac8a5576c57a 90 rcom=2;
DudeHD 0:ac8a5576c57a 91 }
jjokocha 2:0cf2ef636323 92 //switch skroz dolje desno sluzi za Arming drona
DudeHD 0:ac8a5576c57a 93 if(SW3.read()==1){
DudeHD 0:ac8a5576c57a 94 hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",1000,1000,1000,1000,2000,0,0,0);
DudeHD 0:ac8a5576c57a 95 }
jjokocha 2:0cf2ef636323 96
DudeHD 0:ac8a5576c57a 97 else{
jjokocha 2:0cf2ef636323 98 hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,RO,PT,YA,1000,0,0,rcom);
jjokocha 2:0cf2ef636323 99 pc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,RO,PT,YA,1000,0,0,rcom);
jjokocha 2:0cf2ef636323 100 pc.printf("%0.2f\n",vbat);
jjokocha 2:0cf2ef636323 101 pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
DudeHD 0:ac8a5576c57a 102 }
DudeHD 1:4c27d179c6c2 103 //testiranje na pcu
DudeHD 0:ac8a5576c57a 104 //printf("%hd,%hd,%hd,%hd,%hd,%hd\n",x1,y1,x2,y2,sw1.read(),sw2.read());
DudeHD 0:ac8a5576c57a 105 //printf("%hd,%hd,%hd,%hd\n",sw1.read(),sw2.read(),sw3.read(),tog1.read());
DudeHD 1:4c27d179c6c2 106
jjokocha 2:0cf2ef636323 107 vbat=(abat.read()*3.3f)/bat_divider;
DudeHD 1:4c27d179c6c2 108 //ispis na komp
jjokocha 2:0cf2ef636323 109 pc.printf("%0.2f\n",vbat);
DudeHD 1:4c27d179c6c2 110 wait_ms(10);//
DudeHD 0:ac8a5576c57a 111
DudeHD 0:ac8a5576c57a 112 }
jjokocha 2:0cf2ef636323 113
DudeHD 0:ac8a5576c57a 114 else{
DudeHD 1:4c27d179c6c2 115
DudeHD 0:ac8a5576c57a 116 hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",1000,1500,1500,1500,2000,0,0,0);
DudeHD 1:4c27d179c6c2 117 wait_ms(100);
DudeHD 0:ac8a5576c57a 118
DudeHD 0:ac8a5576c57a 119 }
DudeHD 0:ac8a5576c57a 120 }
DudeHD 0:ac8a5576c57a 121 }
DudeHD 1:4c27d179c6c2 122
DudeHD 1:4c27d179c6c2 123 void calibration(){
jjokocha 2:0cf2ef636323 124
DudeHD 1:4c27d179c6c2 125 x1_ar=0;
DudeHD 1:4c27d179c6c2 126 x2_ar=0;
DudeHD 1:4c27d179c6c2 127 x3_ar=0;
DudeHD 1:4c27d179c6c2 128 y1_ar=0;
DudeHD 1:4c27d179c6c2 129 y2_ar=0;
DudeHD 1:4c27d179c6c2 130 y3_ar=0;
jjokocha 2:0cf2ef636323 131
DudeHD 1:4c27d179c6c2 132 for(int a=0;a<cal_num;a++){
DudeHD 1:4c27d179c6c2 133 x1_ar+=a1.read();
DudeHD 1:4c27d179c6c2 134 x2_ar+=a2.read();
DudeHD 1:4c27d179c6c2 135 x3_ar+=a5.read();
DudeHD 1:4c27d179c6c2 136 y1_ar+=a3.read();
DudeHD 1:4c27d179c6c2 137 y2_ar+=a4.read();
DudeHD 1:4c27d179c6c2 138 y3_ar+=a6.read();
DudeHD 1:4c27d179c6c2 139 }
DudeHD 1:4c27d179c6c2 140 x1_cor=mid_point-((x1_ar/cal_num)*100);
DudeHD 1:4c27d179c6c2 141 x2_cor=mid_point-((x2_ar/cal_num)*100);
DudeHD 1:4c27d179c6c2 142 x3_cor=mid_point-((x3_ar/cal_num)*100);
DudeHD 1:4c27d179c6c2 143 y1_cor=mid_point-((y1_ar/cal_num)*100);
DudeHD 1:4c27d179c6c2 144 y2_cor=mid_point-((y2_ar/cal_num)*100);
DudeHD 1:4c27d179c6c2 145 y3_cor=mid_point-((y3_ar/cal_num)*100);
DudeHD 1:4c27d179c6c2 146 //pc.printf("%hd,%hd,%hd,%hd,%hd,%hd\n",x1_cor,y1_cor,x2_cor,y2_cor,x3_cor,y3_cor);
DudeHD 1:4c27d179c6c2 147 }
DudeHD 1:4c27d179c6c2 148
DudeHD 1:4c27d179c6c2 149 void th_calc(){
jjokocha 2:0cf2ef636323 150
jjokocha 2:0cf2ef636323 151 if(abs(TH)<20){
DudeHD 1:4c27d179c6c2 152 TH_def+=0;
DudeHD 1:4c27d179c6c2 153 }
DudeHD 1:4c27d179c6c2 154 else{
jjokocha 2:0cf2ef636323 155 //ako je veca povecaj th def za 10, ako je manja od 0 smanji th def za 10
DudeHD 1:4c27d179c6c2 156 if(TH>0){
DudeHD 1:4c27d179c6c2 157 TH_def+=10;
DudeHD 1:4c27d179c6c2 158 }
DudeHD 1:4c27d179c6c2 159 else{
DudeHD 1:4c27d179c6c2 160 TH_def-=10;
DudeHD 1:4c27d179c6c2 161 }
DudeHD 1:4c27d179c6c2 162 }
DudeHD 1:4c27d179c6c2 163 //sprjecavamo da ode izvan granica
DudeHD 1:4c27d179c6c2 164 if(TH_def<1000)TH_def=1000;
DudeHD 1:4c27d179c6c2 165 if(TH_def>2000)TH_def=2000;
jjokocha 2:0cf2ef636323 166 // u slucaju da osoba stisne gumb na joysticku lijevo th se spusta na 1000 th zaustavlaju se motori
jjokocha 2:0cf2ef636323 167 if(JSW2.read()==1)TH_def=1000;
DudeHD 1:4c27d179c6c2 168 }
jjokocha 2:0cf2ef636323 169
jjokocha 2:0cf2ef636323 170 //Spojio na standardne pinove 14,15 s desne strane ploce mislimd a su to pb9 i pb8
jjokocha 2:0cf2ef636323 171 //onda ko i kog magnetometras idu otpornici sa sda i scl na 3.3v
jjokocha 2:0cf2ef636323 172 //VCC 5 ili 3.3 probaj s 3.3 ak
jjokocha 2:0cf2ef636323 173 //GND gnd
jjokocha 2:0cf2ef636323 174 //SCL scl
jjokocha 2:0cf2ef636323 175 //sda sda
jjokocha 2:0cf2ef636323 176 //eda i ecl na nista
jjokocha 2:0cf2ef636323 177 //AD0 na GND
jjokocha 2:0cf2ef636323 178 //i ova ostala 3 na nista, INT,NCS,FSYNC
jjokocha 2:0cf2ef636323 179
DudeHD 0:ac8a5576c57a 180
jjokocha 2:0cf2ef636323 181 float sum = 0;
jjokocha 2:0cf2ef636323 182 uint32_t sumCount = 0;
jjokocha 2:0cf2ef636323 183
jjokocha 2:0cf2ef636323 184 MPU9250 mpu9250;
jjokocha 2:0cf2ef636323 185
jjokocha 2:0cf2ef636323 186 Timer t;
jjokocha 2:0cf2ef636323 187
jjokocha 2:0cf2ef636323 188 //Serial pc(PA_2, PA_3); // tx, rx
jjokocha 2:0cf2ef636323 189
jjokocha 2:0cf2ef636323 190 volatile bool newData = false;
jjokocha 2:0cf2ef636323 191
jjokocha 2:0cf2ef636323 192 InterruptIn isrPin(D12); //k64 D12 dragon PD_0
jjokocha 2:0cf2ef636323 193
jjokocha 2:0cf2ef636323 194 void mpuisr()
jjokocha 2:0cf2ef636323 195 {
jjokocha 2:0cf2ef636323 196 newData=true;
DudeHD 0:ac8a5576c57a 197 }
DudeHD 0:ac8a5576c57a 198
jjokocha 2:0cf2ef636323 199 void ziroskop()
jjokocha 2:0cf2ef636323 200 {
jjokocha 2:0cf2ef636323 201 pc.baud(9600);
jjokocha 2:0cf2ef636323 202
jjokocha 2:0cf2ef636323 203 //Set up I2C
jjokocha 2:0cf2ef636323 204 i2c.frequency(400000); // use fast (400 kHz) I2C
jjokocha 2:0cf2ef636323 205
jjokocha 2:0cf2ef636323 206 pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
jjokocha 2:0cf2ef636323 207
jjokocha 2:0cf2ef636323 208 t.start();
jjokocha 2:0cf2ef636323 209 isrPin.rise(&mpuisr);
jjokocha 2:0cf2ef636323 210
jjokocha 2:0cf2ef636323 211 // Read the WHO_AM_I register, this is a good test of communication
jjokocha 2:0cf2ef636323 212 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
jjokocha 2:0cf2ef636323 213 pc.printf("I AM 0x%x\n\r", whoami);
jjokocha 2:0cf2ef636323 214 pc.printf("I SHOULD BE 0x71\n\r");
jjokocha 2:0cf2ef636323 215
jjokocha 2:0cf2ef636323 216 if (whoami == 0x71) { // WHO_AM_I should always be 0x68
jjokocha 2:0cf2ef636323 217 pc.printf("MPU9250 is online...\n\r");
jjokocha 2:0cf2ef636323 218 wait(1);
jjokocha 2:0cf2ef636323 219
jjokocha 2:0cf2ef636323 220
jjokocha 2:0cf2ef636323 221 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
jjokocha 2:0cf2ef636323 222 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
jjokocha 2:0cf2ef636323 223
jjokocha 2:0cf2ef636323 224 wait(2);
jjokocha 2:0cf2ef636323 225 mpu9250.initMPU9250();
jjokocha 2:0cf2ef636323 226 mpu9250.initAK8963(magCalibration);
jjokocha 2:0cf2ef636323 227 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
jjokocha 2:0cf2ef636323 228 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
jjokocha 2:0cf2ef636323 229 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
jjokocha 2:0cf2ef636323 230 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
jjokocha 2:0cf2ef636323 231 wait(2);
jjokocha 2:0cf2ef636323 232 } else {
jjokocha 2:0cf2ef636323 233 pc.printf("Could not connect to MPU9250: \n\r");
jjokocha 2:0cf2ef636323 234 pc.printf("%#x \n", whoami);
jjokocha 2:0cf2ef636323 235
jjokocha 2:0cf2ef636323 236
jjokocha 2:0cf2ef636323 237 while(1) ; // Loop forever if communication doesn't happen
jjokocha 2:0cf2ef636323 238 }
jjokocha 2:0cf2ef636323 239
jjokocha 2:0cf2ef636323 240 mpu9250.getAres(); // Get accelerometer sensitivity
jjokocha 2:0cf2ef636323 241 mpu9250.getGres(); // Get gyro sensitivity
jjokocha 2:0cf2ef636323 242 mpu9250.getMres(); // Get magnetometer sensitivity
jjokocha 2:0cf2ef636323 243 pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
jjokocha 2:0cf2ef636323 244 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
jjokocha 2:0cf2ef636323 245 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
jjokocha 2:0cf2ef636323 246 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
jjokocha 2:0cf2ef636323 247 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
jjokocha 2:0cf2ef636323 248 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
jjokocha 2:0cf2ef636323 249
jjokocha 2:0cf2ef636323 250 while(1) {
jjokocha 2:0cf2ef636323 251 static int readycnt=0;
jjokocha 2:0cf2ef636323 252 // If intPin goes high, all data registers have new data
jjokocha 2:0cf2ef636323 253
jjokocha 2:0cf2ef636323 254 #if USE_ISR
jjokocha 2:0cf2ef636323 255 if(newData) {
jjokocha 2:0cf2ef636323 256 newData=false;
jjokocha 2:0cf2ef636323 257 mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS); //? need this with ISR
jjokocha 2:0cf2ef636323 258 #else
jjokocha 2:0cf2ef636323 259 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
jjokocha 2:0cf2ef636323 260 #endif
jjokocha 2:0cf2ef636323 261 readycnt++;
jjokocha 2:0cf2ef636323 262 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
jjokocha 2:0cf2ef636323 263 // Now we'll calculate the accleration value into actual g's
jjokocha 2:0cf2ef636323 264 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
jjokocha 2:0cf2ef636323 265 ay = (float)accelCount[1]*aRes - accelBias[1];
jjokocha 2:0cf2ef636323 266 az = (float)accelCount[2]*aRes - accelBias[2];
DudeHD 0:ac8a5576c57a 267
jjokocha 2:0cf2ef636323 268 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
jjokocha 2:0cf2ef636323 269 // Calculate the gyro value into actual degrees per second
jjokocha 2:0cf2ef636323 270 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
jjokocha 2:0cf2ef636323 271 gy = (float)gyroCount[1]*gRes - gyroBias[1];
jjokocha 2:0cf2ef636323 272 gz = (float)gyroCount[2]*gRes - gyroBias[2];
jjokocha 2:0cf2ef636323 273
jjokocha 2:0cf2ef636323 274 mpu9250.readMagData(magCount); // Read the x/y/z adc values
jjokocha 2:0cf2ef636323 275 // Calculate the magnetometer values in milliGauss
jjokocha 2:0cf2ef636323 276 // Include factory calibration per data sheet and user environmental corrections
jjokocha 2:0cf2ef636323 277 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
jjokocha 2:0cf2ef636323 278 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
jjokocha 2:0cf2ef636323 279 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
jjokocha 2:0cf2ef636323 280 }
jjokocha 2:0cf2ef636323 281
jjokocha 2:0cf2ef636323 282 Now = t.read_us();
jjokocha 2:0cf2ef636323 283 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
jjokocha 2:0cf2ef636323 284 lastUpdate = Now;
jjokocha 2:0cf2ef636323 285
jjokocha 2:0cf2ef636323 286 sum += deltat;
jjokocha 2:0cf2ef636323 287 sumCount++;
jjokocha 2:0cf2ef636323 288
jjokocha 2:0cf2ef636323 289
jjokocha 2:0cf2ef636323 290 // Pass gyro rate as rad/s
jjokocha 2:0cf2ef636323 291 uint32_t us = t.read_us();
jjokocha 2:0cf2ef636323 292 mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
jjokocha 2:0cf2ef636323 293 us = t.read_us()-us;
jjokocha 2:0cf2ef636323 294 int delt_t = 0; // used to control display output rate
jjokocha 2:0cf2ef636323 295 int count = 0; // used to control display output rate
jjokocha 2:0cf2ef636323 296 // Serial print and/or display at 0.5 s rate independent of data rates
jjokocha 2:0cf2ef636323 297 delt_t = t.read_ms() - count;
jjokocha 2:0cf2ef636323 298 if (delt_t > 500) { // update LCD once per half-second independent of read rate
jjokocha 2:0cf2ef636323 299 pc.printf("readycnt %d us %d\n",readycnt,us);
jjokocha 2:0cf2ef636323 300 readycnt=0;
jjokocha 2:0cf2ef636323 301 pc.printf("ax = %f", 1000*ax);
jjokocha 2:0cf2ef636323 302 pc.printf(" ay = %f", 1000*ay);
jjokocha 2:0cf2ef636323 303 pc.printf(" az = %f mg\n\r", 1000*az);
jjokocha 2:0cf2ef636323 304
jjokocha 2:0cf2ef636323 305 pc.printf("gx = %f", gx);
jjokocha 2:0cf2ef636323 306 pc.printf(" gy = %f", gy);
jjokocha 2:0cf2ef636323 307 pc.printf(" gz = %f deg/s\n\r", gz);
jjokocha 2:0cf2ef636323 308
jjokocha 2:0cf2ef636323 309 pc.printf("gx = %f", mx);
jjokocha 2:0cf2ef636323 310 pc.printf(" gy = %f", my);
jjokocha 2:0cf2ef636323 311 pc.printf(" gz = %f mG\n\r", mz);
jjokocha 2:0cf2ef636323 312
jjokocha 2:0cf2ef636323 313 tempCount = mpu9250.readTempData(); // Read the adc values
jjokocha 2:0cf2ef636323 314 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
jjokocha 2:0cf2ef636323 315 pc.printf("temperature = %f C\n\r", temperature);
jjokocha 2:0cf2ef636323 316
jjokocha 2:0cf2ef636323 317 pc.printf("q0 = %f\n\r", q[0]);
jjokocha 2:0cf2ef636323 318 pc.printf("q1 = %f\n\r", q[1]);
jjokocha 2:0cf2ef636323 319 pc.printf("q2 = %f\n\r", q[2]);
jjokocha 2:0cf2ef636323 320 pc.printf("q3 = %f\n\r", q[3]);
jjokocha 2:0cf2ef636323 321
jjokocha 2:0cf2ef636323 322 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]);
jjokocha 2:0cf2ef636323 323 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
jjokocha 2:0cf2ef636323 324 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]);
jjokocha 2:0cf2ef636323 325 pitch *= 180.0f / PI;
jjokocha 2:0cf2ef636323 326 yaw *= 180.0f / PI;
jjokocha 2:0cf2ef636323 327 yaw += 3.95f; // Godine 2018 magnetna deklinacija je +3* 57’ za Zagreb
jjokocha 2:0cf2ef636323 328 roll *= 180.0f / PI;
jjokocha 2:0cf2ef636323 329
jjokocha 2:0cf2ef636323 330 pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
jjokocha 2:0cf2ef636323 331
jjokocha 2:0cf2ef636323 332 count = t.read_ms();
jjokocha 2:0cf2ef636323 333 sum = 0;
jjokocha 2:0cf2ef636323 334 sumCount = 0;
jjokocha 2:0cf2ef636323 335
jjokocha 2:0cf2ef636323 336 }
jjokocha 2:0cf2ef636323 337 }
jjokocha 2:0cf2ef636323 338
DudeHD 0:ac8a5576c57a 339 }
DudeHD 0:ac8a5576c57a 340