po czym jest wyznaczana wartość minimalna i maksymalna pola magnetycznego tylko dla osi X oraz Y ponieważ oś Z nie jest nam potrzebna do dalszych obliczeń. Cykl ten powtarza się przez czas określony zmienną petla. W czasie tym konieczne jest obrócenie HMC5883L co najmniej o jeden pełny obrót wokół osi Z lub więcej, celem odczytania wszystkich możliwych wartości danych magnetometru dla danego terenu w którym się znajduje. Gdy czas na kalibrację się skończy, zostaje obliczone przesunięcie układu współrzędnych tak by jego środek znajdował się pomiędzy maksymalną i minimalną wartością pola magnetycznego odczytanego z czujnika dla osi X oraz Y. Funkcja po wykonaniu tego wszystkiego automatycznie powraca do menu w którym to możemy teraz śmiało włączyć opcję kompasu mając pewność że poprzez kalibrację obniżyliśmy błąd obliczanego kąta użytego do wyświetlenia róży wiatrów w funkcji Opcja_Kompas():
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 |
void Opcja_Kompas(void) { volatile unsigned long int i; signed short x, y, z; float scaledx, scaledy, heading; float declinationAngle = 0.0880; float headingDegrees=0; unsigned char Tekst[4] = {"\0"}; extern signed short offsetx,offsety; volatile signed short k,srednia; volatile int l,xn=0,xs=0,yn=0,ys=0,xw=0,yw=0,xe=0,ye=0; volatile int xns,xss,yns,yss,xws,yws,xes,yes ; volatile int xo1,yo1,xo2,yo2,xo3,yo3,xo4,yo4; volatile int xo1s,yo1s,xo2s,yo2s,xo3s,yo3s,xo4s,yo4s; float stopien = (2 * 3.1415926) / 360; //Rysowanie rozy wiatrow TFTN_Clear(0x000); TFTN_WriteBMP(roza, 0, 0, 132, 132); //Kompas dziala do momentu wcisniecia JOY_DOWN while(GPIO_ReadInputDataBit(JOY_SW_PORT, JOY_DOWN)) { k=(signed short)headingDegrees; //kat opsujacy kierunek geograficzny //Stare wspolrzedne rownaja sie nowym //celem wyczyszczenia starych danych z wyswietlacza xns=xn; yns=yn; xss=xs; yss=ys; xws=xw; yws=yw; xes=xe; yes=ye; xo1s=xo1; yo1s=yo1; xo2s=xo2; yo2s=yo2; xo3s=xo3; yo3s=yo3; xo4s=xo4; yo4s=yo4; //Obliczanie nowych wspolrzednych dla rysowania //N,S,W,E,o,o,o,o w odniesieniu do kąta kierunku geograficznego xn = (int)(62 + 59 * sin(stopien*k)); yn = (int)(60 + 59 * cos(stopien*k)); xo1 = (int)(62 + 59 * sin(stopien*(k+45))); yo1 = (int)(60 + 59 * cos(stopien*(k+45))); xo2 = (int)(62 + 59 * sin(stopien*(k+135))); yo2 = (int)(60 + 59 * cos(stopien*(k+135))); xo3 = (int)(62 + 59 * sin(stopien*(k-45))); yo3 = (int)(60 + 59 * cos(stopien*(k-45))); xo4 = (int)(62 + 59 * sin(stopien*(k-135))); yo4 = (int)(60 + 59 * cos(stopien*(k-135))); xw = (int)(62 + 59 * sin(stopien*(k+90))); yw = (int)(60 + 59 * cos(stopien*(k+90))); xs = (int)(62 + 59 * sin(stopien*(k+180))); ys = (int)(60 + 59 * cos(stopien*(k+180))); xe = (int)(62 + 59 * sin(stopien*(k-90))); ye = (int)(60 + 59 * cos(stopien*(k-90))); //Czyszczenie poprzednich znakow i rysowanie nowych TFTN_WriteXY(" \0" ,xns, yns,0x000,0x000, font2); TFTN_WriteXY("N\0" ,xn, yn,0xF00,0x000, font2); TFTN_WriteXY(" \0" ,xss, yss,0x000,0x000, font2); TFTN_WriteXY("S\0" ,xs, ys,0x00F,0x000, font2); TFTN_WriteXY(" \0" ,xws, yws,0x000,0x000, font2); TFTN_WriteXY("W\0" ,xw, yw,0xFFF,0x000, font2); TFTN_WriteXY(" \0" ,xes, yes,0x000,0x000, font2); TFTN_WriteXY("E\0" ,xe, ye,0xFFF,0x000, font2); TFTN_WriteXY(" \0" ,xo1s, yo1s,0x000,0x000, font1); TFTN_WriteXY("o\0" ,xo1, yo1,0xFFF,0x000, font1); TFTN_WriteXY(" \0" ,xo2s, yo2s,0x000,0x000, font1); TFTN_WriteXY("o\0" ,xo2, yo2,0xFFF,0x000, font1); TFTN_WriteXY(" \0" ,xo3s, yo3s,0x000,0x000, font1); TFTN_WriteXY("o\0" ,xo3, yo3,0xFFF,0x000, font1); TFTN_WriteXY(" \0" ,xo4s, yo4s,0x000,0x000, font1); TFTN_WriteXY("o\0" ,xo4, yo4,0xFFF,0x000, font1); //Wyswietlenie kata na wyswietlaczu TFTN_WriteXY(Tekst ,52, 80,0x000,0x000, font1); TFTN_WriteXY(Tekst ,52, 80,0x0D0,0x000, font1); //Pobranie danych pola magnetycznego z czujnika HMC5883L_GetPosition(&x, &y, &z); //Obliczenie kierunku geograficznego //Skalowanie wartosci z czujnika i przesuniecie //ukladu wspolrzednych scaledx=(x+offsetx)*m_Scale; scaledy=(y+offsety)*m_Scale; //Obliczenie kierunku geograficznego heading = atan2(scaledy, scaledx); heading += declinationAngle; if(heading < 0) heading += 6.2831853; //2 pi if(heading > 6.2831853) heading -= 6.2831853; //Zamiana wyniku na stopnie headingDegrees = heading * 180/3.1415926; //Przeksztalcenie liczby stopni na stringa sprintf((char *)Tekst, "%4d\0", (signed short)headingDegrees); //W czasie pracy kompasu mruga LED 8 GPIO_WriteBit(GPIOB, GPIO_Pin_15, (BitAction)... ...(1-GPIO_ReadOutputDataBit(GPIOB, GPIO_Pin_15))); } } |
