Czujnik IMU 6 DoF LSM6DSL – odczyt danych z akcelerometru i żyroskopu

Czujniki są dla robota tym, czym zmysły dla człowieka. Pozwalają odbierać bodźce z otoczenia i reagować na nie. Jednym z podstawowych czujników stosowanych w robotyce, szczególnie w przypadku obiektów latających UAV (unmanned aerial vehicle), są czujnik orientacji, w tym akcelerometr oraz żyroskop. Dzisiaj postaram się przybliżyć obsługę 6-osiowego czujnika LSM6DSL firmy STMicroelectronics, który pozwala odczytywać dane o przyspieszeniu i prędkości kątowej.

Czym jest akcelerometr i żyroskop?

Akcelerometr, inaczej przyspieszeniomierz, to czujnik, którego zadaniem jest pomiar przyspieszenia badanego elementu poprzez pomiar siły. Mierzy siłę zarówno wynikającą ze zmian ruchu czujnika jak i grawitacji. Pomiar przyspieszenia odbywa się w stosunku do przyspieszenia ziemskiego i dlatego wyrażania jest w jednostkach g (9,81 m/s2) lub mg (mili g).

Żyroskop (lub giroskop) to czujnik, który pozwala na pomiar prędkości kątowej obiektu. Wykorzystuje zasadę zachowania momentu pędu. Prędkość obrotowa wyrażana jest w jednostkach dps (degrees per second, stopni na sekundę), gdzie 360 dps odpowiada 60 rpm (revolutions per minute, obrotów na minutę).

Czujniki stosowane w robotyce nie wykorzystują tradycyjnych elementów ruchomych, a ich pomiar bazuje na pomiarze pojemności elektrycznej tworzonych między masą a podstawą.

Układ LSM6DSL to czujnik IMU wykonany w technologii MEMS. Co właściwie oznaczają te skróty?

  • MEMS (Micro Electro Mechanical System) to małe zintegrowane urządzenia lub systemy, które powstały z połączenia elementów mechanicznych oraz elektrycznych. Są wytwarzane w bardzo małej skali, gdzie podstawowe wymiary wyrażane są w mikrometrach. W tej technologii tworzone są obecnie m.in. różnego rodzaju czujniki np. akcelerometry, żyroskopy czy mikrofony.
  • IMU (Inertial Measurement Unit) to zestaw czujników (akcelerometr i żyroskop), który pozwala na śledzenie orientacji obiektu w dwóch osiach.

LSM6DSL łączy w sobie akcelerometr 3D oraz żyroskop 3D. Oferuje konfigurowalny zakres pomiaru przyspieszenia ±2/±4/±8/±16 g i zakres pomiaru prędkości kątowej ±125/±250/±500/±1000/±2000 dps. Oprócz podstawowych funkcji, układ oferuje liczne dodatkowego możliwości, w tym bufor danych FIFO, możliwość wykrywania swobodnego spadku, uderzeń, czy orientacji. LSM6DSL może także pracować jako urządzenie master i stanowić „mostek” dla innych czujników komunikujących się przez interfejs I2C.

W tym artykule zajmę się podstawową funkcjonalnością czujnika, czyli odczycie danych o przyspieszeniu i prędkości kątowej. Projekt wykonam na bazie płytki ewaluacyjnej NUCLEO-L476RG oraz nakładki X-NUCLEO-IKS01A2.

Opis podstawowych rejestrów czujnika LSM6DSL

Czujnik LSM6DSL to dość rozbudowany układ. Duże możliwości wiążą się jednak z większym poziomem skomplikowania pod względem konfiguracji i obsługi takiego czujnika. Chcąc wykorzystać pełnię możliwości warto zastanowić się, czy tworzyć własną bibliotekę i analizować dokładnie dokumentację, czy lepiej skorzystać z gotowego rozwiązania. Każdy wybór ma swoje wady i zalety.

My jednak zajmiemy się dzisiaj tylko obsługą podstawowej funkcji czujnika, czyli odczytem danych z akcelerometru i żyroskopu. W tym przypadku konfiguracja będzie stosunkowo prosta, dlatego bibliotekę napiszemy samodzielnie. Do tego będzie nam potrzebna dokumentacja, którą znajdziemy na stronie producenta.

Dokumentacja jest dość obszerna jak na tego typu czujniki. Interesujące nam informacje znajdziemy w dziale 9. Register description. Obsługując każdy czujnik warto zapoznać się pobieżnie z całym plikiem, aby zorientować się w oferowanych funkcjonalnościach. Podstawowe ustawienia dotyczące czujnika przyspieszenia znajdziemy w rejestrze CTRL1_XL (adres 0x10).

Aby uruchomić odczyt danych z akcelerometru, musimy wybrać częstotliwość aktualizacji danych (Output Data Rate), czyli bity ODR_XL oraz zakres pomiarowy (Full scale), czyli bity FS_XL. Dokładny opis wyboru ODR przedstawia tabela 52.

Konfiguracja żyroskopu jest realizowana poprzez rejestr CTRL2_G (adres 0x11).

Tutaj analogicznie wybieramy częstotliwość aktualizacji danych (Output Data Rate), czyli bity ODR_G oraz zakres pomiarowy (Full scale), czyli bity FS_G i FS_125.

W pozostałych rejestrach możemy znaleźć takie ustawienia, jak konfiguracja filtrów oraz przerwań. Nam na ten moment nie będą te funkcje potrzebne.

Wiemy w jaki sposób włączyć i skonfigurować podstawowe parametry odczytu z akcelerometru i żyroskopu. Teraz musimy odnaleźć rejestry, z których będziemy mogli odczytać dane. W tym celu przechodzimy do rejestrów OUT (Output), które mają adresy od 0x22 do 0x2D. Jest ich 12 – mamy dwa czujniki, każdy z czujników ma trzy osie, zaś każda oś reprezentowana jest przez 16-bitową wartość (czyli dwa rejestry).

Te podstawowe informacje wystarczą nam do uruchomienia podstawowych funkcji LSM6DSL. Przejdź zatem do konfiguracji projektu.

Konfiguracja mikrokontrolera STM32L476RG

Jak wspomniałem, projekt wykonamy przy użyciu płytki ewaluacyjnej NUCLEO-L476RG oraz nakładki X-NUCLEO-IKS01A2. Do odczytu danych z LSM6DSL potrzebujemy interfejsu I2C lub SPI. Dzisiaj skorzystamy z magistrali I2C.

Schemat płytki X-NUCLEO-IKS01A2 może na pierwszy rzut oka wydawać się skomplikowany. Jednak po krótkiej analizie możemy zauważyć, że czujnik LSM6DSL podłączony jest przy domyślnej konfiguracji zworek do pinów 9 i 10 złącza CN5.

Na płytce NUCLEO-L476RG w tym miejscu wyprowadzone są piny PB8 oraz PB9 mikrokontrolera, na których dostępny mamy interfejs I2C1.

W konfiguratorze STM32CubeMX wybieramy w takim wypadku interfejs I2C1 na pinach PB8 (SCL) oraz PB9 (SDA). Ustawiamy tryb pracy jako I2C i prędkość komunikacji w trybie Standard (100 kHz).

Konfiguracja wyprowadzeń będzie wyglądała jak na grafice poniżej.

I to w zasadzie wszystko. Możemy wygenerować projekt (Alt+K) i przejść do napisania kodu programu.

Implementacja w kodzie programu

Do projektu dodajemy dwa pliki stanowiące naszą bibliotekę: lsm6dsl.c oraz lsm6dsl.h.

W pliku nagłówkowym umieszczamy definicje rejestrów układu LSM6DSL. Dzięki temu program będzie przejrzysty i łatwiej będzie nam operować na nazwach, niż bezpośrednio na adresach rejestrów.

#define LSM6DSL_I2C_ADDR     		    0xD6
#define LSM6DSL_ID            		    0x6A

#define LSM6DSL_WHO_AM_I      		    0x0F
#define LSM6DSL_CTRL1_XL      		    0x10
#define LSM6DSL_CTRL2_G       		    0x11
#define LSM6DSL_CTRL3_C       		    0x12
#define LSM6DSL_CTRL4_C       		    0x13
#define LSM6DSL_CTRL5_C       		    0x14
#define LSM6DSL_CTRL6_C       		    0x15
#define LSM6DSL_CTRL7_G       		    0x16
#define LSM6DSL_CTRL8_XL      		    0x17
#define LSM6DSL_CTRL9_XL      		    0x18
#define LSM6DSL_CTRL10_C      		    0x19

#define LSM6DSL_STATUS_REG                  0x1E

#define LSM6DSL_OUT_TEMP_L                  0x20
#define LSM6DSL_OUT_TEMP_H                  0x21
#define LSM6DSL_OUTX_L_G                    0x22
#define LSM6DSL_OUTX_H_G                    0x23
#define LSM6DSL_OUTY_L_G                    0x24
#define LSM6DSL_OUTY_H_G                    0x25
#define LSM6DSL_OUTZ_L_G                    0x26
#define LSM6DSL_OUTZ_H_G                    0x27
#define LSM6DSL_OUTX_L_XL                   0x28
#define LSM6DSL_OUTX_H_XL                   0x29
#define LSM6DSL_OUTY_L_XL                   0x2A
#define LSM6DSL_OUTY_H_XL                   0x2B
#define LSM6DSL_OUTZ_L_XL                   0x2C
#define LSM6DSL_OUTZ_H_XL                   0x2D

Dodatkowo dodajemy definicje, które ułatwią nam przeliczanie odczytanych wartości z rejestrów na dane wyrażone w jednostach g/mg oraz dps/mdps.

#define LSM6DSL_ACC_SENSITIVITY_FS_2G   	0.061f
#define LSM6DSL_ACC_SENSITIVITY_FS_4G   	0.122f
#define LSM6DSL_ACC_SENSITIVITY_FS_8G   	0.244f
#define LSM6DSL_ACC_SENSITIVITY_FS_16G  	0.488f

#define LSM6DSL_GYRO_SENSITIVITY_FS_125DPS      4.375f
#define LSM6DSL_GYRO_SENSITIVITY_FS_250DPS      8.750f
#define LSM6DSL_GYRO_SENSITIVITY_FS_500DPS      17.500f
#define LSM6DSL_GYRO_SENSITIVITY_FS_1000DPS     35.000f
#define LSM6DSL_GYRO_SENSITIVITY_FS_2000DPS     70.000f

Skąd wzięły się te wartości? Możemy je policzyć samodzielnie, ale łatwiej i szybciej będzie je odczytać z dokumentacji (tabela 3. na stronie 21).

Na podstawie rejestrów tworzymy teraz definicje typów enum, które ułatwią nam posługiwanie się częstotliwościami odświeżania danych oraz zakresem pomiarowym akcelerometru.

typedef enum {
  LSM6DSL_2g       = 0,
  LSM6DSL_16g      = 1,
  LSM6DSL_4g       = 2,
  LSM6DSL_8g       = 3
} lsm6dsl_fs_xl_t;

typedef enum {
  LSM6DSL_XL_ODR_OFF      =  0,
  LSM6DSL_XL_ODR_12Hz5    =  1,
  LSM6DSL_XL_ODR_26Hz     =  2,
  LSM6DSL_XL_ODR_52Hz     =  3,
  LSM6DSL_XL_ODR_104Hz    =  4,
  LSM6DSL_XL_ODR_208Hz    =  5,
  LSM6DSL_XL_ODR_416Hz    =  6,
  LSM6DSL_XL_ODR_833Hz    =  7,
  LSM6DSL_XL_ODR_1k66Hz   =  8,
  LSM6DSL_XL_ODR_3k33Hz   =  9,
  LSM6DSL_XL_ODR_6k66Hz   = 10,
  LSM6DSL_XL_ODR_1Hz6     = 11
} lsm6dsl_odr_xl_t;

Analogicznie definiujemy enumy dla żyroskopu.

typedef enum {
  LSM6DSL_250dps     = 0,
  LSM6DSL_125dps     = 1,
  LSM6DSL_500dps     = 2,
  LSM6DSL_1000dps    = 4,
  LSM6DSL_2000dps    = 6
} lsm6dsl_fs_g_t;

typedef enum {
  LSM6DSL_GY_ODR_OFF    =  0,
  LSM6DSL_GY_ODR_12Hz5  =  1,
  LSM6DSL_GY_ODR_26Hz   =  2,
  LSM6DSL_GY_ODR_52Hz   =  3,
  LSM6DSL_GY_ODR_104Hz  =  4,
  LSM6DSL_GY_ODR_208Hz  =  5,
  LSM6DSL_GY_ODR_416Hz  =  6,
  LSM6DSL_GY_ODR_833Hz  =  7,
  LSM6DSL_GY_ODR_1k66Hz =  8,
  LSM6DSL_GY_ODR_3k33Hz =  9,
  LSM6DSL_GY_ODR_6k66Hz = 10
} lsm6dsl_odr_g_t;

Aby łatwiej było przechowywać dane, stworzymy też unię i strukturę przechowującą odpowiednio surowe dane z rejestrów (unia) oraz przeliczone dane wyrażone w jednostkach g lub dps.

typedef union
{
	uint8_t bytes[LSM6DSL_ACCE_DATA_SIZE];

	struct __attribute__((packed))
	{
	  int16_t x;
	  int16_t y;
	  int16_t z;
	};
}LSM6DSL_AxesRaw_t;

typedef struct
{
  int32_t x;
  int32_t y;
  int32_t z;
} LSM6DSL_Axes_t;

W pliku c. stworzymy kilka funkcji pozwalających na inicjalizację czujnika oraz odczyt danych.

W funkcji init, oprócz konfiguracji parametrów odczytu, ustawimy również w rejestrze CTRL3_C bit odpowiadający za automatyczną inkrementację adresów. Dzięki temu chcąc odczytać dane dotyczące wszystkich osi akcelerometru lub żyroskopu będziemy mogli wykonać to w jednej operacji odczytu, a nie w 6, wysyłając za każdym razem kolejny adres. Dodatkowo na początku funkcji sprawdzamy za pomocą odczytu ID, czy na pewno nasz czujnik to LSM6DSL.

void lsm6dsl_init(void)
{
	uint8_t data = 0;

	lsm6dsl_read(LSM6DSL_WHO_AM_I, &data, 1);

	if(LSM6DSL_ID != data)
	{
		return;
	}

	data = LSM6DSL_SET_ODR_XL(LSM6DSL_XL_ODR_208Hz) | LSM6DSL_SET_FS_XL(LSM6DSL_2g);
	lsm6dsl_write(LSM6DSL_CTRL1_XL, &data, 1);

	data = LSM6DSL_SET_ODR_G(LSM6DSL_GY_ODR_208Hz) | LSM6DSL_SET_FS_G(LSM6DSL_125dps);
	lsm6dsl_write(LSM6DSL_CTRL2_G, &data, 1);

	data = LSM6DSL_SET_AUTO_INC;
	lsm6dsl_write(LSM6DSL_CTRL3_C, &data, 1);
}

Zakres pomiarowy skonfigurowałem na poziomie ±2g oraz ±125dps. Dane będziemy odczytywali co 10 ms, zatem częstotliwość pomiarów na poziomie 208 Hz będzie wystarczająca.

W funkcji inicjalizacyjnej posługuję się kilkoma makrami, które pozwalają w przejrzysty sposób zrealizować przesunięcia bitowe. Warto stosować takie techniki, bo dzięki nimi kod programu jest bardziej zrozumiały.

#define LSM6DSL_SET_ODR_XL(data)	(data << 4)
#define LSM6DSL_SET_FS_XL(data)		(data << 2)

#define LSM6DSL_SET_ODR_G(data)		(data << 4)
#define LSM6DSL_SET_FS_G(data)		(data << 1)

#define LSM6DSL_SET_AUTO_INC		(1 << 2)

Funkcje do odczytu danych z akcelerometru i żyroskopu sprowadzają się do odczytu danych z rejestrów czujnika oraz przeliczenia ich odpowiednio na jednostki wyrażone w mg i mdps.

void lsm6dsl_get_accel_axis(LSM6DSL_Axes_t *axes)
{
	LSM6DSL_AxesRaw_t axes_raw;

	lsm6dsl_read(LSM6DSL_OUTX_L_XL, axes_raw.bytes, LSM6DSL_ACCE_DATA_SIZE);

	axes->x = (int32_t)((float)axes_raw.x * LSM6DSL_ACC_SENSITIVITY_FS_2G);
	axes->y = (int32_t)((float)axes_raw.y * LSM6DSL_ACC_SENSITIVITY_FS_2G);
	axes->z = (int32_t)((float)axes_raw.z * LSM6DSL_ACC_SENSITIVITY_FS_2G);
}

void lsm6dsl_get_gyro_axis(LSM6DSL_Axes_t *axes)
{
	LSM6DSL_AxesRaw_t axes_raw;

	lsm6dsl_read(LSM6DSL_OUTX_L_G, axes_raw.bytes, LSM6DSL_GYRO_DATA_SIZE);

	axes->x = (int32_t)((float)axes_raw.x * LSM6DSL_GYRO_SENSITIVITY_FS_125DPS);
	axes->y = (int32_t)((float)axes_raw.y * LSM6DSL_GYRO_SENSITIVITY_FS_125DPS);
	axes->z = (int32_t)((float)axes_raw.z * LSM6DSL_GYRO_SENSITIVITY_FS_125DPS);
}

Dodatkowo stworzyłem dwie funkcje realizujące odczyt i zapis przez interfejs I2C.

void lsm6dsl_write(uint8_t reg, uint8_t *data, uint32_t size)
{
	HAL_I2C_Mem_Write(I2C_INTERFACE, LSM6DSL_I2C_ADDR, reg, 1, data, size, I2C_TIMEOUT);
}

void lsm6dsl_read(uint8_t reg, uint8_t *data, uint32_t size)
{
	HAL_I2C_Mem_Read(I2C_INTERFACE, LSM6DSL_I2C_ADDR, reg, 1, data, size, I2C_TIMEOUT);
}

Aby przetestować działanie biblioteki, w funkcji main() dodałem kilka linijek kodu, które zrealizują odczyt danych z czujnika.

  lsm6dsl_init();

  uint32_t time_ms = HAL_GetTick();
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
		if((HAL_GetTick() - time_ms) > 10)
		{
			time_ms = HAL_GetTick();
			lsm6dsl_get_accel_axis(&accel_axes);
			lsm6dsl_get_gyro_axis(&gyro_axes);
		}
  }

Struktury accel_axes i gyro_axes zainicjalizowałem jako globalne, dzięki czemu będziemy mogli je podejrzeć np. za pomocą STM32CubeMonitor.

/* USER CODE BEGIN PV */
LSM6DSL_Axes_t accel_axes;
LSM6DSL_Axes_t gyro_axes;
/* USER CODE END PV */

Przykładowy przebieg zarejestrowany w STM32CubeMonitor przedstawia poniższa grafika.

Podsumowanie

Układ LSM6DSL łączy w sobie akcelerometr i żyroskop. Są to jedne z podstawowych czujników stosowanych w robotyce, które używane są zarówno w robotach jeżdżących, jak i latających. Pozwalają na pomiar ruchu obiektu, a dzięki temu są wykorzystywane do określenia orientacji w przestrzeni i utrzymania stabilności lotu.

W artykule przedstawiłem, w jaki sposób odczytać podstawowe parametry z tego typu czujnika oraz jak je przeliczyć na jednostki stosowane w fizyce. Stworzyliśmy własną bibliotekę na podstawie informacji z dokumentacji, dzięki czemu nauczyliśmy się radzić sobie w przypadkach, gdy nie mamy do dyspozycji gotowego przykładu obsługi. Pomimo tego, że na rynku jest wiele rodzajów czujników, inne akcelerometry i żyroskopy konfigurowane są na podobnej zasadzie. Mam nadzieje, że będzie Ci łatwiej podjeść do tematu, jeśli będziesz miał do czynienia z innym układem, niż LSM6DSL.

Jeżeli masz jakieś pytania, spostrzeżenia lub uwagi, zachęcam do zostawienia komentarza pod artykułem.

Do pobrania

  1. Podkreślenia to dziwny styl pisania kodu ? No to nie chciałbym poprawiać kodu po Benderze… 😉

    Rozdzielanie podkreśleniami poszczególnych składników nazw/identyfikatorów szalenie poprawia czytelność !

    Bardzo ciekawa strona, podziwiam wiedzę autora i dziękuję za dzielenie się nią.

    PS
    Co do artykułu – dopiero zaczynam zabawę z IMU i chętnie poczytałbym jak z akcelerometru i żyroskopu przejść na
    fizyczne położenie obiektu w przestrzeni, czyli nudne X,Y i Z w stosunku do jakiegoś umownego początku układu współrzędnych.

    Odpowiedz

Dodaj komentarz

Twój adres e-mail nie zostanie opublikowany. Wymagane pola są oznaczone *