Skocz do zawartości

Żyroskop L3GD20H błędne wskazania


marecki

Pomocna odpowiedź

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 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 zaszedł, 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'

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 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/Conversion_between_quaternions_and_Euler_angles

Sposób wyznaczania błędu spoczynkowego od str 28

https://www.elecrow.com/download/TA0343.pdf

Link do komentarza
Share on other sites

Dołącz do dyskusji, napisz odpowiedź!

Jeśli masz już konto to zaloguj się teraz, aby opublikować wiadomość jako Ty. Możesz też napisać teraz i zarejestrować się później.
Uwaga: wgrywanie zdjęć i załączników dostępne jest po zalogowaniu!

Anonim
Dołącz do dyskusji! Kliknij i zacznij pisać...

×   Wklejony jako tekst z formatowaniem.   Przywróć formatowanie

  Dozwolonych jest tylko 75 emoji.

×   Twój link będzie automatycznie osadzony.   Wyświetlać jako link

×   Twoja poprzednia zawartość została przywrócona.   Wyczyść edytor

×   Nie możesz wkleić zdjęć bezpośrednio. Prześlij lub wstaw obrazy z adresu URL.

×
×
  • Utwórz nowe...

Ważne informacje

Ta strona używa ciasteczek (cookies), dzięki którym może działać lepiej. Więcej na ten temat znajdziesz w Polityce Prywatności.