po czym wyznacza wartość minimalną i maksymalną 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 MAG3110 co najmniej o jeden pełny obrót wokół osi Z lub więcej razy, 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 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 |
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); //Pobranie danych pola magnetycznego z czujnika MAG3110_GetPosition(&x, &y, &z); //Odczytanie temperatury MAG3110_ReadRegister(0x0F,&temp); //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 na stringa dla katu i wyswietlenie sprintf((char *)Tekst, "%3d\0", (signed short)headingDegrees); TFTN_WriteXY("Kat=" ,85, 120,0x000,0xFFF, font1); TFTN_WriteXY(Tekst ,110, 120,0x000,0xFFF, font1); //Przeksztalcenie liczby na stringa dla temperatury oraz wyswietlenie sprintf((char *)Tekst, "%3d\0", (signed char)temp+27); TFTN_WriteXY("T=",5, 120,0x000,0xFFF, font1); TFTN_WriteXY(Tekst,18, 120,0x000,0xFFF, font1); //W czasie pracy kompasu mruga LED 8 GPIO_WriteBit(GPIOB, GPIO_Pin_15, (BitAction)... ...(1-GPIO_ReadOutputDataBit(GPIOB, GPIO_Pin_15))); } } |
