W czasie kalibracji wyznaczane są maksymalne i minimalne wartości pola magnetycznego dla osi „X” oraz „Y” czujnika. Gdy kalibracja skończy się, wyskalowane i przesunięte zostają wartości odczytane z czujnika na środek wirtualnego układu współrzędnych:
|
1 2 3 4 5 6 7 8 9 |
offsetx=((xmin+xmax)*0.5); if (offsetx<0) offsetx=(-1*offsetx); if (xmin<0) xmin=(-1*xmin); if ((xmin)<xmax) offsetx=-offsetx; offsety=((ymin+ymax)*0.5); if (offsety<0) offsety=(-1*offsety); if (ymin<0) ymin=(-1*ymin); if ((ymin)<ymax) offsety=-offsety; |
Następnie pobierane są wartości natężeń pola magnetycznego:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 |
//Pobranie danych pola magnetycznego HMC5883Get(HMC5883File, &x,&y,&z); //Obliczenie kierunku geograficznego //Skalowanie wartosci z czujnika i przesuniecie //ukladu wspolrzednych scaledx=(x+offsetx)*0.73; scaledy=(y+offsety)*0.73; //Obliczenie kierunku geograficznego //heading = atan2(y, x); heading = atan2(scaledy, scaledx); heading += declinationAngle+3.1415926; if(heading < 0) heading += 6.2831853; //2 pi if(heading > 6.2831853) heading -= 6.2831853; //Zamiana wyniku na stopnie headingDegrees = (heading * 180/3.1415926); |
Po tych obliczeniach wyświetlony zostaje wynik w postaci przewijającej się linijki z zaznaczonym kierunkiem geograficznym. Zastosowany efekt przypomina widok sztucznego horyzontu stosowanego w starszych samolotach:
|
1 |
SLCD_Horyzont(pasek,(unsigned short)headingDegrees); |
Funkcja wygląda następująco:
|
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 |
void SLCD_Horyzont(unsigned char *tablica, unsigned short pozycja) { unsigned char i, odnowa=0, poczatek=0; float mnoznik = 0.355555; unsigned short polozenie; polozenie=(unsigned short) (pozycja * mnoznik); for (i=0;i<37;i++) { //jeśli koniec tablicy if (tablica[i+polozenie]==0x00) poczatek=1; if(!poczatek) { for(piksel=0;piksel<8;piksel++) { if((tablica[i+polozenie]&(1<<piksel))!=0) { _SLCDModule_SetSegment(i, piksel); } } } else { //Jeśli koniec tablicy rysuj obraz od początku na dalszej części lcd for(piksel=0;piksel<8;piksel++) { if((tablica[odnowa]&(1<<piksel))!=0) { _SLCDModule_SetSegment(i, piksel); } } odnowa++; } } } |
Funkcja przyjmuje wskaźnik do tablicy z rysunkiem wspomnianej linijki. Gdyby ją wyświetlić na szerszym ekranie wyglądało by to mniej więcej tak:
|-i-N-i-|-i-E-i-|-i-S-i-|-i-W-i-
z czego małe „i” to krótka pionowa kreseczka
Jako, że ekran jest za krótki do wyświetlenia całego paska, funkcja wycina część z tego obrazka oraz – jeśli jest to konieczne – przerysowuje początek tablicy na końcu ekranu w celu wypełnienia brakującego obrazu.
W dalszej części programu main() znajduje się opóźnienie oraz na końcu na obraz zostaje nałożona maska, która wyświetla pierwsze i ostatnie dwie kolumny pikseli oraz kolumnę pośrodku, aby wiadomo było jaki kierunek geograficzny wskazuje układ kompasu. Logo Freescale oraz symbol buzzera pozostają włączone.
|
1 2 3 4 5 6 7 8 9 10 11 12 |
for(czas=0;czas<200000;czas++); LCD_WF3TO0 = 0xFFFF1100; LCD_WF7TO4 = 0x00000000; LCD_WF11TO8 = 0x00000000; LCD_WF15TO12 = 0x00000000; LCD_WF19TO16 = 0x00000000; LCD_WF23TO20 = 0x000000FF; LCD_WF27TO24 = 0x00000000; LCD_WF31TO28 = 0x00000000; LCD_WF35TO32 = 0x00000000; LCD_WF39TO36 = 0x00FFFF00; |
Całość od momentu odczytu pola magnetycznego aż do wyświetlenia kierunku geograficznego powtarza się w pętli.
| Uwaga! W pliku C:\Freescale\Freescale MQX 3.8\mqx\source\io\lcd\slcd_kwikstik-k40x256 jest błąd polegający na tym, że w funkcji void _SLCDModule_SetSegment(U08 X, U08 Y) przed wyświetleniem pojedynczego piksela cała kolumna zostaje skasowana. Aby to zmienić należy wykomentować linię:
|
