Witam,
Wykonuję obsługę odczytu danych z żyroskopu L3GD20H. Dane są wysyłane z uC na komputer za pośrednictwem BTM-222. Transmisja przebiega prawidłowo. Na wejściu dostaję dane w postaci:
-ilość taktów zegara od ostatniego wystawienia nowych danych przez gyro dla F_CPU=8000 000 i preskalera 8 i dla ODR=200 dostaję TCNT1L = 125, TCNT1H=20. Po przeliczeniach wychodzi 190,66 [Hz] więc jest to wartość prawidłowa.
-dane pobrane z gyro mianowicie OUT_X_L, OUT_X_H, OUT_Y_L, OUT_Y_H, OUT_Z_L, OUT_Z_H.
Odbierane dane przy nieruchomym gyro są przetwarzane w procedurze inicjalizacyjnej w następujący sposób:
-pominięcie pięciu pierwszych odczytów - wartości znacznie odbiegające od pozostałych,
-wyznaczenie błędu spoczynkowego - wyliczenie średniej z kolejnych np 200 odczytów danych dla poszczególnych osi,
-wyznaczenie odchylenia standardowego i 3-sigma które jest nam później potrzebne aby rozpoznawać czy nastąpił już obrót czy jeszcze nie,
Po tym wstępie kolejne dane są już traktowane jako dane prawidłowe do wykonania odczytu. Operacje które na nich zachodzą to:
-wyliczenie wartości 16-bitowej odczytanej danej prędkości obrotu,
-obliczenie różnicy pomiędzy wartością zmierzoną a wartością błędu spoczynkowego i porównanie czy nie przekracza wartości 3-sigma, jeśli nie przekracza to ruch nie miał miejsca, jeśli przekracza to wartość bierzemy do dalszych operacji,
-pomnożenie wartości prędkości obrotu przez czułość,
-podzielenie prędkości obrotowej przez czas w którym zachodziła - w wyniku tego mamy kąt obrotu,
-wyznaczenie za pomocą kwaternionów pojedynczej osi obrotu, która zastępuje nam trzy składowe osie, punkt który obracamy wokół osi ma współrzędne początkowe P=(0,0,1) tak więc możemy wyznaczyć nowe położenie punktu P
-obliczenie kąta pomiędzy osią Z=(0,0,1) a nowym położeniem P'
Potrzebuję mierzyć kąt jaki występuje pomiędzy osią pionową skierowaną prostopadle do powierzchni ziemi a osią Z gyro. Ta oś jest równoznaczna z pierwotnym położeniem osi Z gyro. Tak więc jeśli obrócę gyro i dowolne kąty i następnie powrócę do pozycji początkowej to kąt wychyłu powinien wrócić do zera. Wykonuję obliczenie kąta jaki występuje pomiędzy osią (wektorem) globalną Z a osią (wektorem) Z gyro. Położenie końca osi Z gyro jest wyznaczane przez położenie punktu P którego początkowe położenie to (0, 0, 1). Położenie punktu P zmienia się przy każdym obrocie gyro i jest wyznaczane za pomocą kwaternionu.
I tutaj zaczynają się problemy. Odchylam gyro od osi globalnej która jest zorientowana pionowo do powierzchni ziemi (gyro też w położeniu początkowym ma również oś Z zorientowaną pionowo). Jeśli odchylam gyro wokół osi X o 90 stopni i wracam do położenia pierwotnego to wskazanie kąta odchylenia również zmienia się od 0 do 90 i ponownie na 0. Jednak jeśli przechył gyro wykonam względem osi nierównoległej do osi X i Y i następnie powrót to wskazanie nie wraca do zera a np do 30 stopni.
Nie mam już pomysłu gdzie robię błąd. Kwaterniony zastępują nam obroty wokół poszczególnych osi, które trzeba było by rozważać jako najpierw obrót np wokół X, potem Y a na końcu Z, ale jeśli byśmy zmienili na np Y, X i Z to położenie końcowe punktu P było by inne.
Liczba (wartość czasu) przez którą dzielimy prędkość kontową jest również pobierana z gyro więc nie ma tam błędu. Bezruch gyro jest również wykrywany.
Proszę o sugestię gdzie może tkwić błąd w moich rozważaniach. Nie chcę (na razie) stosować żadnych dodatkowych czujników np akcelerometru bo tok rozumowania i działań które przedstawiłem/zaimplementowałem jest sensowny ale przy multi-obrocie nie działa. Na pewno ktoś z tu obecnych również miał takie problemy.
Kod wyznaczania kwaternionu
https://en.wikipedia.org/wiki/Conversio ... ler_anglesSposób wyznaczania błędu spoczynkowego od str 28
https://www.elecrow.com/download/TA0343.pdfKąt pomiędzy wektorami
http://eszkola.pl/matematyka/kat-miedzy ... -5489.html