Dopasowanie osi i kalibracja magnetyczna
Teraz wystarczy tylko dopasować dane czujników do lotniczego układu współrzędnych. Całą pracę z magnetometrami należy wykonywać na drewnianym stole, z dala od jakichkolwiek zasilaczy czy stalowych mebli.
Połóż kompas płasko tak, aby oś z była skierowana w dół. Sprawdź, czy akcelerometr pokazuje około 1 g na osi z, a zero na osiach x i y. Odwróć kompas tak, aby oś z była skierowana do góry i upewnij się, że odczyt jest bliski -1g. Powtórz procedurę z osiami x oraz y skierowanymi w dół i w górę, aby sprawdzić, czy odczyty wynoszą odpowiednio 1 lub -1. Kilkadziesiąt mg odchylenia od oczekiwanej wartości nie jest istotne, gdyż w tym momencie ważna jest tylko korekta niepoprawnej orientacji układu czujnika obróconego o wielokrotność 90 stopni. Ewentualne poprawki będą dotyczyły tylko tego konkretnego projektu płytki i sprowadzają się do zamiany odczytu dwóch osi lub odwrócenia odczytywanej wartości. Zapisz te poprawki w kodzie i więcej ich nie zmieniaj.
Rys. 3. Punkty dopasowania osi akcelerometru
Rysunek 3 pomoże zrozumieć problem. Akcelerometr mierzy zarówno grawitację, jak i przyspieszenie liniowe. W przypadku braku przyspieszenia (na przykład urządzenie leżące na stole), wskazanie akcelerometru będzie zawsze równe 1 g, a zatem niezależnie od orientacji wynik znajdzie się na powierzchni sfery o promieniu 1. Wymienione wcześniej 6 punktów pomiarowych zawsze tworzy ośmiościan wpisany w sferę o promieniu 1 g, a mapowanie osi po prostu obraca ten ośmiościan tak, aby kanały akcelerometru były odpowiednio dopasowane.
Ustawienie osi magnetometru jest podobne do akcelerometru, ale uwzględnia wektor pola ziemskiego. Połóż kompas płasko, skierowany na północ, a następnie obróć wokół osi o 270 stopni, wskazując kolejno wschód, południe i zachód. Odczyt na kanale z powinien być maksymalny, gdy kompas jest skierowany na północ, a minimalny, gdy pokazuje południe. Odczyt y powinien być minimalny w kierunku wschodnim, a minimalny w kierunku zachodnim. Wartość z nie powinna się zmieniać, gdyż jest stała niezależnie od odchylenia.
Odwróć kompas na drugą stronę i powtórz cały proces. Teraz również maksimum wskazania x powinno znajdować się na północy, a minimum – na południu. Wskazanie y powinno być przeciwne – maksymalne w kierunku wschodnim, minimalne – zachodnim. Wskazanie na osi z pozostanie stałe, ale ponieważ oś jest teraz skierowana przeciwnie do składowej pionowej pola magnetycznego (na północnej półkuli), wartość ta będzie mniejsza.
Na rysunku 4 pokazano wyniki pomiarów wykonane podczas obrotu magnetometru znajdującego się na północnej półkuli o 270 stopni, w pozycji normalnej i odwróconej. Minimalne i maksymalne wskazania osi x oraz y wypadają na oczekiwanych kątach, natomiast zmierzona wartość z jest mniejsza po odwróceniu układu na drugą stronę. Zatem w tym magnetometrze osie są prawidłowo zorientowane. Jeśli wyniki będą inne, należy dokonać odpowiedniej korekty poprzez odpowiednią zamianę lub odwrócenie znaku w kodzie programu.
Rys. 4. Wyniku pomiaru za pomocą magnetometru w pozycji normalnej (a) i odwróconej (b)
Z rysunku 4 można wywnioskować wiele informacji. Wyniki wskazań z oraz y leżą na okręgu o promieniu około 25 uT, z czego wynika, że właśnie taką wartość ma pole magnetyczne w płaszczyźnie poziomej. Pomiary jednak są przesunięte względem zera za sprawą pola ferromagnetyku, które jest powodowane przez zarówno materiały ferromagnetyczne na płytce drukowanej, jak i błędem stałym wnoszonym przez sam czujnik. Producenci układów już dawno zdali sobie sprawę, że dokładna kalibracja ich magnetometrów jest bezcelowa, gdyż każdy z nich pracuje w smartfonie w innym polu ferromagnetyków. Kalibrację magnetometru wykonuje się zatem po instalacji na płytce drukowanej, uwzględniając jej pole magnetyczne. W tej chwili wystarczy zauważyć, że odchylenie w przykładowym magnetometrze wynosi 215 i 185 uT. Prosta metoda określania trzech składowych pola ferromagnetyków jest opisana dalej.
Obliczenie kątów przechylenia i pochylenia
W stanie spoczynku akcelerometr zorientowany tak, tak opisano wcześniej (leżący płasko, wierzchem do góry i skierowany na północ) pokazuje po prostu wartość przyspieszenia ziemskiego g wzdłuż osi z skierowanej do dołu. Dla zadanych kątów ?, ?, ? odczyt akcelerometru jest wynikiem przemnożenia tego stanu przez trzy macierze obrotu Rx, Ry oraz Rz, jak pokazują wzory (1).
(1)
Warto zaznaczyć, że powyższe wzory zakładają brak przyspieszenia i opisują jedynie obrót w polu grawitacyjnym. Jest to sensowne założenie w przypadku smartfonów, gdyż użytkownicy będą trzymać urządzenie nieruchomo, aby odczytać wskazanie kompasu. Jednak np. w przypadku samolotu sterowanego radiem, który wykonuje akrobacje pod dużym przeciążeniem, a nawet w przypadku potrząsania kompasem, wskazania będą błędne.
Nie powinno być zaskoczeniem to, że równanie (1) jest niezależne od obrotu wokół osi z. Żaden akcelerometr nie wykrywa obrotu wokół osi poziomej. Gdyby akcelerometry były w stanie wykryć obrót w polu grawitacyjnym, nie potrzebowalibyśmy magnetometru do wyznaczenia wskazania kompasu.
Ze wzoru (1) można łatwo wyprowadzić kąt przechylenia (?) i pochylenia (?), mając dane odczyty akcelerometru Gx, Gy oraz Gz. Proste przekształcenia prowadzą do wzorów (2) oraz (3):
(2)
(3)
Zakres zmienności kąta przechylenia (?) to od -180 do 180 stopni, co wymaga użycia funkcji C atan2 – w listingu 1 służy do wyznaczenia (?). Zakres kąta pochylenia (?) to od -90 do 90 stopni, zatem do obliczenia (?) wystarczy standardowa funkcja C atan. Nie należy upraszczać tych wzorów, na przykład wyznaczając (?) jako arcsin(-Gx). Nawet w stanie spoczynku szum czujnika może sprawić, że Gx czasem przekroczy zakres od -1 do 1 g, wskutek czego wynik funkcji arcsin będzie bezsensowny. Funkcje arctan w listingu 1. są natomiast stabilne, gdyż zarówno atan, jak i atan2 przyjmują dowolne argumenty od minus do plus nieskończoności.
Następnym krokiem jest zakodowanie równań (2) oraz (3), pamiętając, że w większość funkcji matematycznych zwraca kąt w radianach, które trzeba przemnożyć przez 180/?, równe około 57,3, aby uzyskać wynik w stopniach. Połóż kompas na stole i upewnij się, że obliczone przechylenie i pochylenie są bliskie 0. Obróć kompas do góry i sprawdź, że obliczony kąt pochylenia jest dodatni, osiąga wartość 90, gdy kompas jest skierowany pionowo do góry i maleje do zera, gdy kompas jest odwrócony. Powtórz to samo, kierując kompas do dołu – teraz obliczone pochylenie powinno być ujemne. Następnie obróć kompas wzdłuż osi podłużnej i sprawdź, czy wskazanie przechylenia (?) jest dodatnie, wynosi 90 stopni, gdy kompas znajduje się na brzegu i 180 stopni, gdy jest odwrócony spode do góry. Powtórz ten test, obracając kompas w lewą stronę – tym razem wyznaczony kąt powinien być ujemny. Jeśli okaże się, że kąty są zamienione lub odwrócone, należy wrócić do procedury dopasowania.
Wyznaczanie kierunku wskazywanego przez kompas
Podobnie jak w równaniu (1), odczyt magnetometru B zależy od obrotu pola magnetycznego ziemi oraz dodanego wektora pola magnetyków twardych V. Tą zależność przedstawia wzór (4):
(4)
Po odjęciu wektora V od obu stron równania oraz pomnożeniu przez odwrotność macierzy przechylenia i pochylenia dostajemy zależność (5):
(5)
Składowe x oraz y równania (5) zostały wymienione poniżej w równaniu (6):
(6)
Stosunek wartości y oraz x zależy od kąta wskazywanego przez kompas z kompensacją przechylenia (7):
(7)
Znając przechylenie (?) i pochylenie (?) dzięki akcelerometrowi, możemy za pomocą równania (7) wyznaczyć azymut (?) pod warunkiem, że znamy też wpływ interferencji opisywanych wektorem V. Po odjęciu V od obu stron równania (4) i obliczeniu kwadratu uzyskanych wielkości otrzymujemy równanie (8). Lewa strona wynika z twierdzenia Pitagorasa, natomiast prawą stronę można uprościć, zauważając, że wartość pola ziemskiego B0 pozostaje niezależna od obrotu.
(8)
Wzór (8) mówi, że przy dowolnym obrocie magnetometru wokół własnej osi, odczyty magnetometru leżą na powierzchni sfery o promieniu B0 i środku zależnym od pola ferromagnetyków Vx, Vy, Vz. Szybką i skuteczną techniką jest obrócenie magnetometru wokół każdej z osi, zapisanie minimalnych i maksymalnych wskazań magnetometru, na następnie dokonanie kalibracji na podstawie średnich odczytów (wzór 9):
(9)
Wzór (9) dostarcza kompletu informacji potrzebnych we wzorze (7) do wyznaczenia azymutu (?). Czytelnicy zainteresowani bardziej zaawansowanym algorytmem, korzystającym z metody najmniejszych kwadratów do celów kalibracji, powinni zapoznać się z notą aplikacyjną Freescale Semiconductor AN4246: „Calibrating an eCompass in the presence of Hard and Soft-Iron Interference”. Na stronie Freescale jest też dodatkowy kod źródłowy w C oraz dalsze szczegóły.
Kod w listingu 1. liczący 15 linii w C realizuje kompletny eCompass z kompensacją wychylenia. Czytelnicy chcący dodać cyfrowy filtr dolnoprzepustowy, aby zmniejszyć wahania obliczanych kątów, powinni pamiętać o użyciu arytmetyki modulo 360 stopni, aby nie uśredniać kątów przy przejściu z –179 lub 179 stopni (południe) na 0 stopni (północ).
