Pamiętam jak na rynek weszła Nokia N95, która była ogromnym skokiem technologicznym. Posiadała ona wbudowany akcelerometr dzięki któremu m. in. interfejs samodzielnie zmieniał orientacje. Dzisiaj już chyba na nikim to nie robi wrażenia, ale w 2007 roku aplikacja z odgłosami miecza świetlnego wydobywającymi się pod wpływem machania telefonem zwaliła mnie z nóg. Dzisiaj akcelerometry obecne są w wielu urządzeniach. I to nie tylko akcelerometr bo są to potężne i rozbudowane jednostki IMU(Inertial Measurement Unit) posiadające nie – jak w przypadku akcelerometru – 3 stopnie swobody(przyśpieszenie w osiach X, Y i Z), ale mające 6, 9, a w połączeniu z barometrem nawet 10 stopni swobody oznaczane również jako XDoF(X – ilość stopni).

MPU6050

Dzisiejszym pacjentem będzie układ MPU6050 firmy InvenSense. Jest to układ IMU o sześciu stopniach swobody. Za sprawą wbudowanego żyroskopu rozszerza on podstawowe 3 stopnie  pochodzące z akcelerometru o kolejne 3 wynikające z żyroskopu. Zatem z jego pomocą możemy mierzyć przyśpieszenie(akcelerometr) oraz prędkość kątową(żyroskop) wokół każdej osi. Czujnik ten jest jest układem MEMS (Microelectromechanical System) co oznacza, że oprócz elektroniki posiada on w swojej strukturze również elementy mechaniczne(ruchome).

Moduł z układem MPU6050 możesz kupić tutaj: MPU6050

Parametry

Układ jest bardzo mały. Wymiary to jedynie 4 x 4 x 0.9 mm dzięki czemu idealnie nadaje się do zastosowań w niewielkich urządzeniach jak smartfony, zegarki czy drony. MPU6050 posiada wbudowanych łącznie sześć 16-bitowych przetworników dla każdej z mierzonej wartości. Mierzone wartości przyśpieszeń i prędkości obrotowej są ustalane programowo. Akcelerometr może dokonywać pomiaru w granicach ±2g, ±4g, ±8g lub ±16g natomiast żyroskop ±250, ±500, ±1000, ±2000°/sec(inaczej dps – degrees per second).

Układ posiada wbudowaną 1024-bajtową kolejkę FIFO. Dzięki niej można rzadziej czytać z układu i w większym stopniu oszczędzać energię jeżeli urządzenie działa na baterii.

Aby odciążyć mikrokontroler który będzie pobierał dane z akcelerometru, MPU6050 posiada wbudowany ‘procesor ruchu’ DMP (Digital Motion Process). Jednostka ta pozwala chociażby na detekcję ruchu lub upadku swobodnego jak i wiele innych przydatnych rzeczy. Niestety użytkownik nie ma do niej bezpośrednio dostępu. Nawet dokumentacja nie za wiele mówi o DMP. W Internecie jest wiele rezultatów inżynierii wstecznej, które umożliwiają skorzystać z części ficzerów DMP. Aby cieszyć się w pełni wpomaganiem procesora ruchu, należy skorzystać z bibliotek dostarczanych przez InvenSense.

Jeśli chodzi o parametry elektryczne to zasilanie tolerowane przez układ mieści się w zakresie 2,375÷3,46 V. Interfejs poprzez który można porozmawiać z IMU to I²C. Co ciekawe sam układ umożliwia pracę logiki I²C na napięciu już od 1,71 V. Chińskie moduły jednak nie pozwalają na to – wszystkie piny zasilania układu są podłączone do 3,3 V.

Ciekawostką jest wbudowany czujnik temperatury, którego możemy zwyczajnie w świecie użyć.

Jeszcze jedną ciekawą rzeczą jest możliwość dołączania do MPU6050 kolejnych urządzeń I²C. Scalak bowiem potrafi komunikować się z innymi układami i zbierać z nich dane. Można wykorzystać np. zewnętrzny magnetometr/kompas i dzięki temu dołożyć kolejne trzy stopnie swobody. Możemy jednocześnie obsłużyć cztery różne urządzenia I²C poprzez zewnętrzne linie I²C MPU6050.

Więcej danych jak np. pobory prądu w różnych warunkach i konfiguracjach znajdziesz w dokumentacji:  MPU60X0 Datasheet.pdf

Schemat i konfiguracja

Dzisiaj skorzystam z płytki Nucleo z mikrokontrolerem STM32F401RE. Mój moduł z układem MPU6050 to popularny GY-521. Schemat połączeniowy jest następujący:

Konfiguracja Cube’a jest prosta – I²C oraz przerwanie zewnętrzne na pinie PA6. I²C można przyśpieszyć do 400 kHz. Przerwanie ściągnąłem zewnętrznym rezystorem do masy, a w konfiguracji wybrałem bez wewnętrznego podciągnięcia. Można oczywiście użyć wewnętrznych. UART, LED i TEST są do celów testowych i debugowych.

Kod

Kod można w zasadzie podzielić na 4 sekcje:

  • Konfiguracja akcelerometru
  • Odczyt mierzonych wartości
  • Kongifuracja przerwań
  • Ukryte funkcje DMP

W części konfiguracyjnej standardowo znajduje się funkcja inicjalizująca urządzenie.

1
void MPU6050_Init(I2C_HandleTypeDef *hi2c);

Resetuje ona akcelerometr do wartości domyślnych, ustawia zegar taktujący układ oraz wyznacza zakresy pomiarowe przyśpieszenia i prędkości kątowej na ±2g i ±250°/sec.

Oprócz niej za konfiguracje odpowiada szereg fukncji. Nazwy w sumie mówią za siebie. Można zresetować MPU6050, ustawić zegary, uśpić lub wyłączyć poszczególne czujniki. W razie potrzeb można też zwiększyć zakres pomiarowy akcelerometru i żyroskpu.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
//
// PWR_MGMT_1
//
void MPU6050_DeviceReset(uint8_t Reset);
void MPU6050_SetClockSource(uint8_t Source);
void MPU6050_SetSleepEnabled(uint8_t Enable);
void MPU6050_SetCycleEnabled(uint8_t Enable);
void MPU6050_SetTemperatureSensorDisbled(uint8_t Disable);

//
// PWR_MGMT_2
//
void MPU6050_SetLowPowerWakeUpFrequency(uint8_t Frequency);
void MPU6050_AccelerometerAxisStandby(uint8_t XA_Stby, uint8_t YA_Stby, uint8_t ZA_Stby);
void MPU6050_GyroscopeAxisStandby(uint8_t XG_Stby, uint8_t YG_Stby, uint8_t ZG_Stby);

//
// Measurement scale configuration
//
void MPU6050_SetFullScaleGyroRange(uint8_t Range);
void MPU6050_SetFullScaleAccelRange(uint8_t Range);
<p style="text-align: justify;">

Chyba najważniejszą częścią z punktu widzenai użytkownika jest odczyt danych z akcelerometru. Przygotowałem kilka fukncji zwracających dane w różnych wariantach. Funkcje z dopiskiem RAW zwracają bezpośrednio to, co znajduje się w rejestrach. Fukcje Scaled zwracają przeskalowane wartości względem maksymalnego zakresu, czyli bezpośrednio w jednostkach przyśpieszenia grawitacyjnego, prędkości kątowej w stopniach na sekundę, lub stopniach Celsjusza. Ostatnią funkcją jest zwracanie wartości Pitch i Roll, czyli krótko mówiąc odchylenia w osi Y i X względem ziemi. Mam tutaj jedną uwagę dotyczącą temperatury. Moim zdaniem MPU6050 trochę zawyża pomiary. Tak 3-4ºC. Jednak nie modyfikowałem formuły, którą podaje datasheet.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
int16_t MPU6050_GetTemperatureRAW(void);
float MPU6050_GetTemperatureCelsius(void);

int16_t MPU6050_GetAccelerationXRAW(void);
int16_t MPU6050_GetAccelerationYRAW(void);
int16_t MPU6050_GetAccelerationZRAW(void);
void MPU6050_GetAccelerometerRAW(int16_t* x, int16_t* y, int16_t* z);
void MPU6050_GetAccelerometerScaled(float* x, float* y, float* z);

int16_t MPU6050_GetRotationXRAW(void);
int16_t MPU6050_GetRotationYRAW(void);
int16_t MPU6050_GetRotationZRAW(void);
void MPU6050_GetGyroscopeRAW(int16_t* x, int16_t* y, int16_t* z);
void MPU6050_GetGyroscopeScaled(float* x, float* y, float* z);

void MPU6050_GetRollPitch(float* Roll, float* Pitch);

W sekcji konfiguracji przerwań znaczną część zajmują funkcje dotyczące pinu INT. Wybrać można na przykład poziom jakim informuje o przerwaniu oraz sposób czyszczenia flagi przerwania. Akcelerometr oferuje standardowo trzy przerwania:

  • Przepełnienie FIFO
  • Przerwanie od Master I²C
  • Przygotowane dane do odczytu

FIFO nie zajmowałem się na tą chwilę. Tak samo funkcją Master I²C. Przerwanie DataReady można powiedzieć, że jest prawie zawsze aktywne gdyż akcelerometr standardowo ciągle generuje dane.

1
2
3
4
5
6
7
8
9
10
11
12
13
//
// Setting INT pin
//
// INT_PIN_CFG register
void MPU6050_SetInterruptMode(uint8_t Mode);
void MPU6050_SetInterruptDrive(uint8_t Drive);
void MPU6050_SetInterruptLatch(uint8_t Latch);
void MPU6050_SetInterruptLatchClear(uint8_t Clear);
// INT_ENABLE register
void MPU6050_SetIntEnableRegister(uint8_t Value);
void MPU6050_SetIntDataReadyEnabled(uint8_t Enable);
// INT_STATUS register
uint8_t MPU6050_GetIntStatusRegister(void);

Więc po jakiego czorta nam te przerwanie? Otóż MPU6050 ma wbudowane DMP które nie jest opisane w dokumentacji czujnika ani w mapie rejestrów. Pewnie dlatego, że InvenSense dostarcza swój przepastny sterownik w tym celu. Internet oczywiście nie śpi i kilka rzeczy rozwiązał. Dzięki temu mamy prosty dostęp do podstawowych funkcji procesora ruchu.

Digital Motion Process

DMP daje nam trzy bardzo użyteczne przerwania. Są to:

  • Detekcja spadku swobodnego
  • Wykrycie ruchu
  • Wykrycie bezruchu

Układ sam potrafi przetworzyć dane z czujników i wystawić odpowiednie przerwanie. Wszystkie 3 zamieściłem w bibliotece. W okół nich jest jeszcze kilka opcji do ustawienia.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
//
// Motion functions - not included in documentation/register map
//
uint8_t MPU6050_GetMotionStatusRegister(void);

void MPU6050_SetDHPFMode(uint8_t Dhpf);
// INT_ENABLE register
void MPU6050_SetIntZeroMotionEnabled(uint8_t Enable);
void MPU6050_SetIntMotionEnabled(uint8_t Enable);
void MPU6050_SetIntFreeFallEnabled(uint8_t Enable);

void MPU6050_SetMotionDetectionThreshold(uint8_t Threshold);
void MPU6050_SetMotionDetectionDuration(uint8_t Duration);

void MPU6050_SetZeroMotionDetectionThreshold(uint8_t Threshold);
void MPU6050_SetZeroMotionDetectionDuration(uint8_t Duration);

void MPU6050_SetFreeFallDetectionThreshold(uint8_t Threshold);
void MPU6050_SetFreeFallDetectionDuration(uint8_t Duration);

Pierwsza funkcja to odczyt rejestru statusowego odpowiedzialnego za wykryty ruch. W nim znajduje się informacja o tym, w którym kierunku ruch wywołał przerwanie MotionDetect. Być może komuś się przyda. Ja na chwile obecną nie widzę u siebie zastosowania.

1
2
3
4
5
6
7
8
9
10
<p style="text-align: justify;"> //
// Not documented
// Register 91 - Motion Status
//
#define MPU6050_MOTION_MOT_XNEG_BIT 7
#define MPU6050_MOTION_MOT_XPOS_BIT 6
#define MPU6050_MOTION_MOT_YNEG_BIT 5
#define MPU6050_MOTION_MOT_YPOS_BIT 4
#define MPU6050_MOTION_MOT_ZNEG_BIT 3
#define MPU6050_MOTION_MOT_ZPOS_BIT 2</p>

Druga z funkcji jest to ustawienie filtra górnoprzepustowego dla DMP. Nie zauważyłem wpływu na działanie przerwań. Może ktoś zna więcej szczegółów na temat tego filtra? Zalecam ustawić na wartość MPU6050_DHPF_5 tak jak ja.

No i pozostają funkcje odpowiadające za wykrywanie zdarzeń. Po pierwsze, można użyć je w dwojaki sposób. Z pinem INT oraz bez czytając cyklicznie status przerwań. Nie widzę powodu dla którego miałbym odpytywać MPU o status przerwań. Lepiej niech to będzie wykonywało się  sprzętowo, a to aktywujesz odpowiednimi funkcjami.

Dalej są ustawienia warunków wystąpienia przerwania. Threshold to próg zmiany przyśpieszenia przy którym zacznie się zliczanie wewnętrznego licznika. Duration to wartość tego licznika po której przekroczeniu zostanie wystawione przerwanie. Gdy wartość przyśpieszenia spadnie poniżej progu przez przerwaniem, licznik zeruje się.

Próg wyrażony jest w mg(miligie), a wpisana wartość jest mnożona razy 2. Czas trwania natomiast jest w milisekundach. Czyli wpisując Threshold = 2, Duration = 5 do rejestrów MotionCapture przerwanie nastąpi po 5 milisekundach od przekroczenia różnicy 4 mg w porównaniu do stanu spoczynku. Może to brzmieć nieco skomplikowanie dlatego polecam poeksperymentować. Przetestowałem wszystkie przerwania i na pewno działają. Stuknięcie w biurko wyzwala odpowiednia Motion ZeroMotion.

Upuszczenie czujnika wygeneruje przerwanie Freefall.

Dryft żyroskopu

Niestety układy IMU jak wszystko w elektronice są nieidealne. Najbardziej idzie to odczuć na żyroskopie. Posiada on coś takiego jak dryft, czyli wskazuje ciągle jakąś wartość mimo, że stoi bez ruchu. Widać to na poniższym screenie z terminala.

MPU6050, który mam na biurku ma dosyć spory dryft w osi X. Z niego wynika, że w ciągu około 180 sekund wykonuje pełem obrót w okół X. Powiem Ci, że patrzę na niego i jak dla mnie to on się na tym biurku nie kręci ani trochę 🙁

Jednym ze sposobów na minimalizację tego błędu to filtracja. Najskuteczniejszy tutaj jest filtr Kalmana, o którym będzie osobny wpis.

Podsumowanie

Układy MEMS są bardzo ciekawymi tworami. Dzięki nim można w łatwy sposób “wielkości mechaniczne” przenieść w świat elektroniki i dzięki temu prosto obrabiać. Gdyby nie akcelerometry nie byłoby dronów czy okularów VR.

Dzięki wbudowanemu DMP można zaoszczędzić wielu cykli CPU lub nawet wykorzystać przerwania do fajnych celów. Widzę np. usypianie i budzenie smartwatch’a na podstawie wykrywanego ruchu/bezruchu. Jest napewne wiele innych zastosowań tego rodzaju detekcji. Podziel się swoimi pomysłami w komentarzach.

Kod standardowo znajdziesz na moim GitHubie: link

Jeśli zauważyłeś jakiś błąd, nie zgadzasz się z czymś, chciałbyś coś dodać istotnego lub po prostu uważasz, że chciałbyś podystkutować w tym temacie, napisz komentarz. Pamiętaj, że dyskusja ma być kulturalna i zgodna z zasadami języka polskiego.

6 stopni swobody z MPU6050 na STM32
5 (100%) 3 votes

Podobał Ci się artykuł? Oceń wpis i dołącz do mojego newslettera!

Podziel się wpisem

8 Komentarzy

Marcin · 07/03/2019 o 17:53

A co myślisz o obliczaniu przemieszczenia na podstawie przyspieszenia?

Odczytujesz co kwant czasu przyspieszenie, obliczasz z niego drogę i kierunek, a na podstawie żyroskopu określasz pozycję obrotową IMU w przestrzeni.

    Mateusz · 07/03/2019 o 18:31

    Pewnie chodzi Ci o coś takiego: https://www.youtube.com/watch?v=6ijArKE8vKU

    Fajny temat. Pewnie trzeba trochę policzyć pochodnej, ale nie jest to wielki wyczyn nawet dla MCU. Sam nie próbowałem jeszcze, ale może kiedyś 🙂

      Marcin · 07/03/2019 o 22:59

      Trafiłeś w sedno, to mi chodziło po głowie.
      I da się, więc robię, albo szukam gotowca 🙂

      Mateusz · 08/03/2019 o 08:30

      Nie szukałem żadnych przykładów kodu, ale nie wydaje się to jakieś bardzo skomplikowane. Daj znać jak Ci wyszło 🙂

MyIden · 07/03/2019 o 16:58

Ciekawe, wkrótce wezmę się za ten czujniczek … mam już PCB przez chińczyków staranie wykonane do sterownika silnika BLDC do e-bike – z założenia chcę aby akcelerometr wykrywał hamowanie = wyłączenie silnika ( brak będzie czujnika na korbie – jedynie tensometr w ośce silnika ), pochylenie boczne roweru = wyłączenie silnika, oraz pochylenie przód tył – automatyczna zmiana poziomu wspomagania .. zobaczymy ja wyjdzie.

    Mateusz · 07/03/2019 o 18:33

    Trzymam kciuki! Wiele jeździków elektrycznych posiada IMU, więc nie powinno być większego problemu z własną implementacją w rowerze.

Krystian · 06/03/2019 o 21:13

Czekam na filtr Kalmana 🙂

    Mateusz · 06/03/2019 o 22:16

    Na pewno będzie 🙂

Dodaj komentarz

Twój adres email nie zostanie opublikowany. Pola, których wypełnienie jest wymagane, są oznaczone symbolem *

Serwis wykorzystuje pliki cookies. Korzystając ze strony wyrażasz zgodę na wykorzystywanie plików cookies. więcej informacji

Wrażenie zgody na pliki Cookies jest konieczne, aby uzyskać najlepsze wrażenia z przeglądania strony. Jeżeli nadal nie wyraziłeś zgody na używanie plików Cookies, zaakceptuj poniżej klikając w przycisk "Akceptuj" na banerze.

Close