SantyagoSantyago
Avatar

Witaj!
Blog archiwalny. Już niebawem nowy serwis!

YouTube RSS Facebook GitHub

Arduino poradnik

Wstęp

Teoria

Biblioteki

Komponenty

Czujniki i sensory

Rozwiązania i algorytmy

Narzędzia

Mikrokontrolery i Arduino IDE

Arduino i klony

Poradniki wideo

Reklama na Blogu

Najnowsze poradniki

Ostatnie komentarze

Ostatnie fotografie

polskie-gorypolskie-gorypolskie-gorypolskie-gorypolskie-gorypolskie-gorypolskie-gorypolskie-gorypolskie-gorywieliczka-szyb-danilowicza

3-osiowy żyroskop L3G4200D

Żyroskop to urządzenie służące do pomiaru lub utrzymania położenia kątowego, który działa w oparciu o zasadę zachowania momentu pędu. Układ L3G4200D stanowi osobną grupę żyroskopów prędkościowych, które nie utrzymują stałego kierunku, ale wskazują prędkość kątową obiektu, na którym się znajduje. Żyroskopy są głównie wykorzystywane do budowy żyrokompasów, stosowanych przy budowie samobalansujących robotów oraz multicopterów.

Żyroskop sam w sobie nie znajdzie zbyt dużego zastsowania, jednak idealnie nadaje się do wspomagania pomiarów z innych czujnków  np.: akcelerometrów, które oprócz wskazania przyśpieszenia obiektu względem wybranej osi, dają możlwiość określenia jego konfiguracji trzech osi: nachylenia (Pitch), przechylenia (Roll) oraz obrotu (Yaw). Praktycznie wyznaczenie tych osi jest możliwe za pomocą samego żyroskopu, jednak może okazać się niewystarczające.

Innym problemem jest wyznaczenie parametrów Pitch, Roll i Yaw za pomocą samego akcelerometru, który w miarę dokładnie poradzi sobie z wyznaczeniem tylko pierwszych dwóch (Pitch i Roll) - o ile nasz obiekt nie będzie się przemieszczał. Kompletnie nie nada się natomiast do wyznaczenia parametru Yaw. Tak jak wspomniałem wcześniej - idealnym rozwiązaniem jest uwzględenienie pomiarów zarówno z akcelerometru i żyroskopu, który skompensuje błędy wynikające z ruchu.

Pomimo wykorzystania akcelerometru i żyroskopu, kłopotliwy okaże się wciąż Yaw, który docelowo powinno się określać za pomocą magnetometru (kompasu), ale również z uwzględnieniem pomiarów z akcelerometru. Koniec, końców - najlepsze rezultaty dadzą wszystkie trzy czujniki. Do tego tematu powrócę jeszcze w artykułach o akcelerometrach i magnetometrach oraz testu inercyjnych zespółów pomiarowy IMU.

Wróćmy jednak do naszego żyroskopu. Zobaczmy jak przedstawiają się wybrane osie obrotów względem samego układu.

Jeśli chodzi o parametry L3G4200D to pozwala on na wybór jednego z trzech zakresów pomiarowych: ±250°/s, ±500°/s lub ±2000°/s. Czujnik obsługuje zarówno komunikację I2C jak i SPI, jednak najczęściej spotkamy się modułami przystosowanymi jedynie do magistrali  I2C

L3G4200D posiada również  wyjścia cyfrowe (INT1 i INT2) mogące sygnalizować (w zależności od konfiguracji rejestrów) pojawienie się nowego pomiaru, przepełnienie FIFO lub zbyt niskiej/wysokiej prędkości obrotu. Układ może być zasilany napięciem z zakresu 2,4 - 3.6 V, natomiast logika we/wy do 1,71 V. Typowy pobór prądu podczas pomiaru to zaledwie 6mA. Należy zwrócić więc szczególną uwagę czy nasz moduł będzie tolerował zasilanie 5V, gdyż nieodpowiedni jego poziom może uszkodzić układ.

Pełna dokumentacja techniczna: https://www.jarzebski.pl/datasheets/L3G4200D.pdf

Osobiście posiadam moduł IMU GY-80, który pozwala na zasilanie napięciem 5V. Pin oznaczony SCL (adapter) podłączamy do pinu A5 (Arduino), natomiast pin SDA (adapter) do pinu A4 (Arduino).

Biblioteka i program testowy

Niestety - przeszukując sieć, nie natrafiłem na w żadną dobrą bibliotekę dla Arduino, dlatego zbierając szczątkowe implementacje, postanowiłem napisać własną. W odróżnieniu od dostępnych bibliotek, moja biblioteka pozwala na kalibrację układu w spoczynku (do kompensacji późniejszych pomiarów), wybór zakresu pomiarowego i normalizowanie danych, a także ustawienie współczynnika progu czułości. Oczywiście jest to pierwsza wersja, więc na pewno będzie jeszcze rozwiajana. Biblioteka posiada również przykłady wykorzystania oraz program do wizualizacji pomiarów. Bibliotekę można pobrać stąd: https://github.com/jarzebski/Arduino-L3G4200D

  1. #include <Wire.h>
  2. #include <L3G4200D.h>
  3.  
  4. L3G4200D gyroscope;
  5.  
  6. void setup()
  7. {
  8.   Serial.begin(9600);
  9.  
  10.   // Inicjalizacja L3G4200D
  11.   // 250 dps: L3G4200D_250DPS
  12.   // 500 dps: L3G4200D_500DPS
  13.   // 2000 dps: L3G4200D_2000DPS
  14.   while(!gyroscope.begin(L3G4200D_2000DPS))
  15.   {
  16.     Serial.println("Nie odnaleziono L3G4200D. Sprawdz polaczenie.");
  17.     delay(500);
  18.   }
  19.  
  20.   // Sprawdzamy skalę
  21.   Serial.print("Wybrana skala: ");
  22.  
  23.   switch(gyroscope.getScale())
  24.   {
  25.     case L3G4200D_SCALE_250DPS:
  26.       Serial.println ("250 dps");
  27.       break;
  28.     case L3G4200D_SCALE_500DPS:
  29.       Serial.println ("500 dps");
  30.       break;
  31.     case L3G4200D_SCALE_2000DPS:
  32.       Serial.println ("2000 dps");
  33.       break;
  34.   }
  35.  
  36.   // Kalibracja żyroskopu. Powinna odbywać się w spoczynku zerowym
  37.   gyroscope.calibrate();
  38.  
  39.   // Ustawiamy próg czułości na 3.
  40.   gyroscope.setThreshold(3);
  41. }
  42.  
  43. void loop()
  44. {
  45.   // Odczytujemy surowe dane z zyroskopu
  46.   Vector raw = gyroscope.readRaw();
  47.  
  48.   // Odczytujemy znormalizowane wyniki w °/s
  49.   Vector norm = gyroscope.readNormalize();
  50.  
  51.   // Wyświetlamy wyniki
  52.   Serial.print(" Xraw = ");
  53.   Serial.print(raw.XAxis);
  54.   Serial.print(" Yraw = ");
  55.   Serial.print(raw.XAxis);
  56.   Serial.print(" Zraw = ");
  57.   Serial.print(raw.ZAxis);
  58.   Serial.print(" Xnorm = ");
  59.   Serial.print(norm.XAxis);
  60.   Serial.print(" Ynorm = ");
  61.   Serial.print(norm.YAxis);
  62.   Serial.print(" ZNorm = ");
  63.   Serial.print(norm.ZAxis);
  64.  
  65.   Serial.println();
  66. }

Wynik działania

Wizualizacja w Processing.org

Lekko zmodyfikowany program przedstawia się następująco:

  1. #include <Wire.h>
  2. #include <L3G4200D.h>
  3.  
  4. L3G4200D gyroscope;
  5.  
  6. int LED = 13;
  7. boolean Blink = false
  8.  
  9. void setup()
  10. {
  11.   Serial.begin(9600);
  12.   pinMode(LED, OUTPUT);
  13.  
  14.   // Inicjalizacja L3G4200D
  15.   while (!gyroscope.begin(L3G4200D_SCALE_2000DPS))
  16.   {
  17.     if (Blink)
  18.     {
  19.       digitalWrite(LED, HIGH);
  20.     } else
  21.     {
  22.       digitalWrite(LED, LOW);
  23.     }
  24.  
  25.     Blink = !Blink;
  26.  
  27.     delay(500);
  28.   }
  29.  
  30.   digitalWrite(LED, HIGH);
  31.  
  32.   // Kalibracja żyroskopu. Powinna odbywać się w spoczynku zerowym
  33.   gyroscope.calibrate();
  34.  
  35.   // Ustawiamy próg czułości na 3.
  36.   gyroscope.setThreshold(3);
  37.   digitalWrite(LED, LOW);
  38. }
  39.  
  40. void loop()
  41. {
  42.   // Odczytujemy znormalizowane wyniki w °/s
  43.   Vector  norm = gyroscope.readNormalize();
  44.  
  45.   // Output
  46.   Serial.print(norm.XAxis);
  47.   Serial.print(":");
  48.   Serial.print(norm.YAxis);
  49.   Serial.print(":");
  50.   Serial.print(norm.ZAxis);
  51.   Serial.println();
  52. }

Program dla Processing znajdziecie w archwium biblioteki, dzięki któremu możemy obserwować wyniki pomiarów. Dla ułatwienia stopnie zostały zamienione na radiany:

Materiały dodatkowe

Biblioteka L3G4200D: https://github.com/jarzebski/Arduino-L3G4200D
Dokumentacja techniczna: https://www.jarzebski.pl/datasheets/L3G4200D.pdf

Reklama

Komentarze Komentarze
Avatar 1
winnikeu Windows / Safari 537.36
20 October 2015 - 01:54 Wrocław

Czy miałeś do czynienia z "yaw drift"?

Avatar 2
Korneliusz Linux Ubuntu / Mozilla Firefox 41.0
24 October 2015 - 02:34 Bytom

Tak miałem - zainteresuj się fitrlem Kalmana :)

Avatar 1
Sylwek Mac OS X / Safari 601.4.4
22 February 2016 - 14:49 Brak informacji

Cześć!

Serdeczne dzięki za opisy i biblioteki - świetna robota.

Używam Twoich bibliotek przy tworzeniu układu sterującego do quadcoptera. HW to NodeMCU + GY-80, komunikacja po I2C.

Niestety wartości zwracane przez żyroskop w w funkcji readNormalize() są nieprawidłowe gdy podłączę GY-80 do NodeMCU, a są w porządku, gdy testuję ten sam kod na Arduino Leonardo.

Masz jakiś pomysł, co może być przyczyną?

Z góry dzięki za pomoc.

Pozdrawiam,
Sylwek.

Avatar 1
Sylwek Windows / Safari 537.36
22 February 2016 - 20:44 Warszawa

Dodam jeszcze, że ściągnąłem inne biblioteki do tego żyroskopu i działają tak samo - z Arduino wszystko jest ok, a z NodeMCU coś się dzieje niedobrego.

Poniżej kilka linijek z portu szeregowego, jaki wychodzi z programu przygotowanego do działania z napisanym przez Ciebie programem do processingu, wyświetlający pomiary w takiej kolejności:

Serial.print(gyroNorm.XAxis);
Serial.print(":");
Serial.print(gyroNorm.YAxis);
Serial.print(":");
Serial.print(gyroNorm.ZAxis);
Serial.print(":");
Serial.print(pitch);
Serial.print(":");
Serial.print(roll);
Serial.print(":");
Serial.println(yaw)

0.70:4587.03:0.49:19999.69:20458.61:6.51
4586.96:4587.38:2.24:20045.56:20504.48:6.53
4587.24:0.56:1.05:20045.56:20550.35:6.54
4587.17:4587.45:0.91:20091.44:20596.22:6.55
0.21:4586.82:1.19:20137.31:20596.22:6.57
4587.24:4587.17:1.40:20183.18:20642.10:6.58
4586.96:0.07:0.98:20183.18:20687.97:6.59
4586.75:4587.10:1.19:20229.05:20733.83:6.6

Zyroskop jest w spoczynku, czyli widać, że te dane są albo szumem, albo bardzo wzmocnione.

Pozdrawiam,
Sylwek.

Avatar 2
Korneliusz Linux Ubuntu / Mozilla Firefox 43.0
22 February 2016 - 20:45 Bytom

Pojęcia nie mam - może sposób zapisywania danych. Jak dorwę NodeMCU to zerknę. Używasz do tego ArduinoIDE?

Avatar 1
Sylwek Windows / Safari 537.36
22 February 2016 - 20:50 Warszawa

Normalnie używam Atmel Studio, ale jak przestało działać to sprawdzałem też w ArduinoIDE.

Myślę, że coś będzie z częstotliwością, na której działa NodeMCU (80 lub 160 MHz).

Inne czujniki z płytki GY-80 działają poprawnie, więc nawet nie myślałem o tym, że błędy będą po stronie komunikacji po I2C, ale może trzeba to sprawdzić, ponieważ NodeMCU jest 32 bitowy.

Tak, czy inaczej - bardzo dziękuję za chęć wsparcia! Jak coś się urodzi fajnego z mojego projektu to się tutaj podzielę ze wszystkimi.

Pozdro!

Avatar 1
Sylwek Windows / Safari 537.36
22 February 2016 - 21:33 Warszawa

Już wiem, gdzie jest rozbieżność.

W funkcji readRaw() odczytywane są wartości z rejestru L3G4200D_REG_OUT_X_L

uint8_t yla = Wire.receive();
uint8_t yha = Wire.receive();

i następnie łączone w jendą wartość:

r.YAxis = yha << 8 | yla;

Gdy czujnik jest w spoczynku, dostaję takie wartości:

yla - ok 250
yha - ok 250

Po połączeniu w r.YAxix otrzymujemy:

- ok (minus)-7 dla Arduino
- ok 65 000 dla NodeMCU.

Jeszcze nie doszedłem do tego, co powinno tam być, i czy odczyt na Y 255 w obu rejestrach jest poprawny, gdy czujnik jest w spoczynku, ale nie chciałem, żebyś tracił czas na szukanie problemu.

Avatar 2
Korneliusz Linux Ubuntu / Mozilla Firefox 43.0
27 February 2016 - 01:12 Bytom

Dzięki wielkie, powodem mogą być typy zmiennych

Avatar 1
Sylwek Windows / Safari 537.36
28 February 2016 - 22:59 Brak informacji

Cześć.

Próbowałeś odpalić ten czujnik na NodeMCU?
Ja cały czas walczę i wydaje mi się, że czegoś nie rozumiem do końca, bo cały czas dostaję na wyjściu jakieś śmieci.

Napisałem najprostszy program testowy, który można też podpiąć pod program do processingu od Ciebie:

setup(){
Serial.begin( 115200 );
while( !Serial ){}

while(!gyroscope.begin(L3G4200D_SCALE_2000DPS, L3G4200D_DATARATE_400HZ_50) ){
Serial.println( " ERROR! 3");
delay( 100 );
}
// gyroscope.calibrate(100);
// gyroscope.setThreshold(3);
}

void loop(){
timer = millis();
Vector gyroNorm = gyroscope.readNormalize(); // Read normalized values in deg/sec

pitch = pitch + gyroNorm.YAxis * timeStep;
roll = roll + gyroNorm.XAxis * timeStep;
yaw = yaw + gyroNorm.ZAxis * timeStep;

Serial.print(gyroNorm.XAxis);
Serial.print(" : ");
Serial.print(gyroNorm.YAxis);
Serial.print(" : ");
Serial.println(gyroNorm.ZAxis);
Serial.print(":");
Serial.print(pitch);
Serial.print(":");
Serial.print(roll);
Serial.print(":");
Serial.println(yaw);

// Wait to full timeStep period
delay((timeStep*1000) - (millis() - timer));
}

Jeżeli zakomentuję poniższe linie kodu:
// gyroscope.calibrate(100);
// gyroscope.setThreshold(3);

To na wyjściu dostaję dla X prawie zawsze ok 4500, dla Y zmienia się z 0 na 4500 co chwilę (nie ruszając czujnikiem), dla Z ok 0.

Gdy odkomentuję te linie, to dla X i Y będzie zawsze 0 - nawet jak ruszam czujnikiem, a dla Z będzie się zmieniało z 0 na 4500 podczas ruszania czujnikiem.

Nie mam pojęcia gdzie jest problem - może coś z tą kalibracją, może ustawienia w funkcji begin, których nie jestem w 100% pewien.

Będę wdzięczny za jakąkolwiek pomoc.

Pozdrawiam,
Sylwek.

Avatar 1
Sylwek Windows / Safari 537.36
01 March 2016 - 23:43 Warszawa

Kolejne info z mojej strony.

Widzę, że różnica jest w samym odczycie rejestrów przez funkcję readRaw().

Przy podłączeniu do Arduino kolejne odczyty są do siebie w miarę zbliżone, przez co, gdy obliczamy deltę na podstawie np 100 pomiarów, to po odjęciu jej od aktualnego pomiaru otrzymujemy wartość bliską zeru, co jest pożądane.

W przypadku podłączenia z NodeMCU, wartości odczytane przez tą samą funkcję, w tych samych warunkach będą się znacznie od siebie różniły. W moim przypadku, gdy czujnik leży na biurku dostaję dla X i Y odczyty wahające się od 0 do 65 000. Średnia wyliczona z takich pomiarów wychodzi dla X ok 35 000, a dla Y ok 50 000. Przez to, dostajemy na wyjściu bardzo dziwne wartości.

Pytanie brzmi, dlaczego ten sam kod, wykonywany przez dwa różne urządzenia odczytuje z rejestrów czujnika różne wartości?

Avatar 1
Sylwek Windows / Safari 537.36
02 March 2016 - 00:59 Warszawa

W końcu mi się udało :)

"Błąd" był w funkcji readRaw() podczas przypisywania odczytów z dwóch rejestrów do zmiennej typu float:

r.XAxis = xha << 8 | xla;

dla xha mnijeszego od 128 wszystko jest ok. Problem pojawia się, gdy xha jest pomiędzy 128 a 255.

r.Axis w przypadku NodeMCU był liczbą od 0 (dla xla = 0 i xha =0) do 65535 (dla xla = 255 i xha = 255) - czyli dokładnie tak, jak się spodziewałem.

W przypadku Arduino był natomiast liczbą od -32768 (dla xla = 0 i xha = 128) do 32767( dla xla = 255 i xha = 127) ALE NIE ROSŁO LINIOWO. Rosło od 0 do 32767 wraz ze wzrostem xha do 127, a później, gdy xha przekraczało 128 to nagle r.Axis spadało do -32768 i rosło aż do -1 dla xha = 255 i xla = 255.

Tak wygląda kawałek kodu odpowiadający za przypisanie wartości dwóch rejestrów do jednej zmiennej.

if( xha < 128 ){
r.XAxis = xha << 8 | xla;
}else{
r.XAxis = ( xha << 8 | xla ) - 65535;
}

Analogiczną operację należy przeprowadzić dla osi Y i Z.

Co ciekawe, nie bez znaczenia jest zasilanie - przy zasilaniu GY-80 bezpośrednio z NodeMCU 3.3V możemy zauważyć, że pomiary nie są dokładne. Należy więc zasilać układ zewnętrznym zasilaniem 5V i przez konwerter napięc podłączyć go do NodeMCU.

Avatar 1
Shadow Windows XP / Mozilla Firefox 50.0
30 November 2016 - 16:18 Brak informacji

Witam mam problem podczas kompilacji pierwszego powyższego programu na arduino.
Dokładnie wyskakuje mi błąd "L3G4200D\' does not name a type"
Co z tym zrobić?

Avatar 1
Speranza Windows / Safari 537.36
30 October 2017 - 19:28 Brak informacji

Hello, I\'m using your library in order to measure from MPU6050 the yaw angle , but now i would like to use two gyroscopes. Can you help me? the library is not made to handle two or more gyroscope.