Mac
Marcin
Grzegorz
KrisS
Frigos
itepe
YCE 72
Luciano
github INA226 project help
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
- #include <Wire.h>
- #include <L3G4200D.h>
- L3G4200D gyroscope;
- void setup()
- {
- Serial.begin(9600);
- // Inicjalizacja L3G4200D
- // 250 dps: L3G4200D_250DPS
- // 500 dps: L3G4200D_500DPS
- // 2000 dps: L3G4200D_2000DPS
- while(!gyroscope.begin(L3G4200D_2000DPS))
- {
- Serial.println("Nie odnaleziono L3G4200D. Sprawdz polaczenie.");
- delay(500);
- }
- // Sprawdzamy skalę
- Serial.print("Wybrana skala: ");
- switch(gyroscope.getScale())
- {
- case L3G4200D_SCALE_250DPS:
- Serial.println ("250 dps");
- break;
- case L3G4200D_SCALE_500DPS:
- Serial.println ("500 dps");
- break;
- case L3G4200D_SCALE_2000DPS:
- Serial.println ("2000 dps");
- break;
- }
- // Kalibracja żyroskopu. Powinna odbywać się w spoczynku zerowym
- gyroscope.calibrate();
- // Ustawiamy próg czułości na 3.
- gyroscope.setThreshold(3);
- }
- void loop()
- {
- // Odczytujemy surowe dane z zyroskopu
- Vector raw = gyroscope.readRaw();
- // Odczytujemy znormalizowane wyniki w °/s
- Vector norm = gyroscope.readNormalize();
- // Wyświetlamy wyniki
- Serial.print(" Xraw = ");
- Serial.print(raw.XAxis);
- Serial.print(" Yraw = ");
- Serial.print(raw.XAxis);
- Serial.print(" Zraw = ");
- Serial.print(raw.ZAxis);
- Serial.print(" Xnorm = ");
- Serial.print(norm.XAxis);
- Serial.print(" Ynorm = ");
- Serial.print(norm.YAxis);
- Serial.print(" ZNorm = ");
- Serial.print(norm.ZAxis);
- Serial.println();
- }
Wynik działania
Wizualizacja w Processing.org
Lekko zmodyfikowany program przedstawia się następująco:
- #include <Wire.h>
- #include <L3G4200D.h>
- L3G4200D gyroscope;
- int LED = 13;
- boolean Blink = false
- void setup()
- {
- Serial.begin(9600);
- pinMode(LED, OUTPUT);
- // Inicjalizacja L3G4200D
- while (!gyroscope.begin(L3G4200D_SCALE_2000DPS))
- {
- if (Blink)
- {
- digitalWrite(LED, HIGH);
- } else
- {
- digitalWrite(LED, LOW);
- }
- Blink = !Blink;
- delay(500);
- }
- digitalWrite(LED, HIGH);
- // Kalibracja żyroskopu. Powinna odbywać się w spoczynku zerowym
- gyroscope.calibrate();
- // Ustawiamy próg czułości na 3.
- gyroscope.setThreshold(3);
- digitalWrite(LED, LOW);
- }
- void loop()
- {
- // Odczytujemy znormalizowane wyniki w °/s
- Vector norm = gyroscope.readNormalize();
- // Output
- Serial.print(norm.XAxis);
- Serial.print(":");
- Serial.print(norm.YAxis);
- Serial.print(":");
- Serial.print(norm.ZAxis);
- Serial.println();
- }
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
Czy miałeś do czynienia z "yaw drift"?
Tak miałem - zainteresuj się fitrlem Kalmana :)
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.
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.
Pojęcia nie mam - może sposób zapisywania danych. Jak dorwę NodeMCU to zerknę. Używasz do tego ArduinoIDE?
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!
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.
Dzięki wielkie, powodem mogą być typy zmiennych
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.
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?
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.
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ć?
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.
