Elektroda.pl
Elektroda.pl
X

Wyszukiwarki naszych partnerów

Wyszukaj w ofercie 200 tys. produktów TME
Kategoria: Kamery IP / Alarmy / Automatyka Bram
Montersi
Proszę, dodaj wyjątek elektroda.pl do Adblock.
Dzięki temu, że oglądasz reklamy, wspierasz portal i użytkowników.

IMU 9DOF na STM32L476-DISCOVERY

Kwarcu 23 Lut 2017 17:09 8211 9
  • IMU 9DOF na STM32L476-DISCOVERY

    WSTĘP

    Inercyjna jednostka pomiarowa o 9 stopniach swobody do określania orientacji w przestrzeni.

    To mały projekt czysto edukacyjny, a jego celami były:
    -przypomnienie sobie jak sie pisze aplikacje na PC w Qt Creator
    -zapoznanie się z FreeRTOS i jego implementacja na procesorze
    -zapoznanie się ze stosunkowo nową rodziną procesorów STM32L4

    HARDWARE
    Płytka STM32L476-DISCOVERY posiada 3-osiowy żyroskop L3GD20 oraz 3-osiowy akcelerometr i magnetometr LSM303C. Komunikacja pomiędzy procesorem a czujnikami odbywa się przez interfejs SPI, ż tym że z L3GD20 to SPI full-duplex a LSM303C SPI half-duplex. Komunikacja z komputerem realizowana jest przez wirtualny port szeregowy wbudowany w ST-LINK/v2-1.

    SOFTWARE (uC)
    Soft na procesor został napisany języku C w środowisku Eclipse z wtyczką SW4STM32, bez bibliotek HAL czy SPL. Jak wspomniałem wcześniej jako system czasu rzeczywistego wykorzystałem FreeRTOS'a (wiem że niepotrzebny do tak małego projektu, powód - patrz "wstęp"). W systemie pracują głównie 3 zadania:
    1)odbiór przez USART (odbieranie komend z komputera)
    2)nadawanie przez USART
    3)odczyt z czujników MEMS
    USART współpracuje z DMA, odbieranie znaków wykorzystuje bufor cykliczny, natomiast dane do nadania wrzucane są do kolejki i wysyłane też przez DMA do USART. CRC nie stosowałem. Dane z czujników próbkowane i wysyłane są z okresem 16ms .
    Biblioteki do obsługi czujników MEMS to nieco przerobione pliki od ST dołączane do paczki STM32Cube.

    SOFTWARE (PC)
    Framework to standardowy Qt Creator, program napisany w C++. Program odbiera "czyste" nieobrobione dane z czujników, potem skaluje je w zalezności od wybranej czułości i pokazuje na "wyświetlaczach" LCD. Następnie za pomocą filtru Madgwicka/Mahony wykonywana jest fuzja danych z czujników i konwersja na kwaternion. Dalej z kwaternionu wyliczane są kąty Eulera (roll, pitch, yaw) oraz macierz rotacji potrzebna do wizualizacji orientacji sześcianu. Dzięki zastosowaniu magnetometru możliwe jest określenie bezwzględnej pozycji obiektu w przestrzeni. Istotna jest kalibracja magnetometru pod kątem "soft&hard iron distortion" aby uzyskać dokładne wyniki. Filtr Madgwick/Mahony potrzebuje danych z czujnikow których osie zorientowane są w systemie NED (North-East-Down). Ja przyjąłem następują orientacje kierunków na płytce: przód płytki(gniazdo mini-USB) zwrócony jest na N(na kostce X), prawa strona na E(na kostce Y), natomiast spód wskazuje na D(na kostce Z). Wizualizacja kostki 3D wykorzystuje bibliotekę OpenGL (mocno wzorowałem się na przykładzie "cube" dostepnym w Qt)


    Link


    Kody źródłowe w załącznikach, może komuś się przydadzą.


    Fajne!
  • #2 24 Lut 2017 09:30
    KeinXor
    Poziom 24  

    Czy kolega może zamieścić binarne wersje programy i wsadu do procesora.

  • #3 24 Lut 2017 16:37
    Kwarcu
    Poziom 11  

    KeinXor napisał:
    Czy kolega może zamieścić binarne wersje programy i wsadu do procesora.


    Dodałem do załączników. Nie gwarantuję że apka Qt będzie działać na innych systemach niż win 7, bo nie testowałem.

  • #4 27 Lut 2017 17:29
    KeinXor
    Poziom 24  

    Witam.
    W wolnej chwili wrzuciłem Twoją skompilowaną binarke na stm32l476g-disco wszystko zadziałało od szczału (program na PC również), jest jednak małe „ale” wizualizacja jest z dość widocznym lagiem nawet kilkusekundowym. Wspomniałeś coś o kalibracji magnetometru czy twój soft obsługuje ową funkcjonalność ? Tak wiem że udostępniłeś źródła to miło ale szczerze nie chce mi się ich analizować.

  • #5 28 Lut 2017 01:21
    rb401
    Poziom 29  

    KeinXor napisał:
    wizualizacja jest z dość widocznym lagiem nawet kilkusekundowym.


    Faktycznie, opóźnienie jest i rośnie w czasie. I przyczyną okazuje się być to że odświeżanie tych "LCD" i sześcianu wywoływane z funkcji odbierania znaków z COMa, nie wyrabia się czasie. I rośnie kolejka w buforze COM. Ale co znaczące, chwilowe przerwanie transmisji np. resetem discovery, nie opróżnia tej kolejki. Bo funkcja jest tylko wywoływana przychodzeniem nowych danych. Tak że nawet małe niewyrobienie w czasie obsługi pojedynczego kompletu danych, będzie się nieodwracalnie kumulować.
    Jeśli te wywołania wyłączyć w źródłach i zostawić tylko prezentację kątów, to komunikacja i obróbka danych się wyrabia na bieżąco i widać jak ładnie i szybko reaguje na ruch. Oczywiście problem dotyczy nieco wolniejszych komputerów, bo jak widać u autora, pełny program w pewnych warunkach się wyrabia.
    Tak że ogólnie jak chodzi o zasadę działania i algorytmy to fajna rzecz i działa, nie licząc tych drobnych kłopotów, bo to już tylko kwestia sposobu zbudowania programu np. wielowątkowo.

  • #6 28 Lut 2017 08:24
    sweter_007
    Poziom 13  

    Kwarcu napisał:
    ...Dzięki zastosowaniu magnetometru możliwe jest określenie bezwzględnej pozycji obiektu w przestrzeni....

    Magnetometr umożliwia określenie bezwzględnej orientacji obiektu w przestrzeni, pozycja w przestrzeni to trochę inna para kaloszy. Pozdr

  • #7 28 Lut 2017 20:06
    Kwarcu
    Poziom 11  

    1. Jeśli chodzi o "lagi" (ja u siebie nie zaobserwowałem) to możliwą przyczyną jest mój błąd w odczycie danych z serialPort:
    Zamiast:

    Kod: c
    Zaloguj się, aby zobaczyć kod

    powinno być:
    Kod: c
    Zaloguj się, aby zobaczyć kod

    wtedy program powinien opróżnić cały bufor, a nie tak jak przedtem odczytać tylko jedną linię, niezależnie od tego ile jest ich do przeczytania.
    Wrzucam w załączniku poprawione binarki, możecie sprawdzić czy to coś poprawiło. Jak pisałem wcześniej jestem na etapie uczenia sie Qt więc apka idealna nie jest ;)

    2.
    KeinXor napisał:
    Wspomniałeś coś o kalibracji magnetometru czy twój soft obsługuje ową funkcjonalność ?

    Jest zaimplementowana korekcja odczytów magnetometru na podstawie macierzy współczynników i offsetu - ale te wartości musiałbyś sam sobie wyznaczyć dla twojego czujnika. Ja korzystałem z tego: Link
    Generalnie, po resecie płytka powinna przez ok 3 s pozostać bez ruchu (liczenie offsetu żyro) i na płaskiej powierzchni (offset X, Y akcelero).

    3.
    sweter_007 napisał:
    Kwarcu napisał:
    ...Dzięki zastosowaniu magnetometru możliwe jest określenie bezwzględnej pozycji obiektu w przestrzeni....

    Magnetometr umożliwia określenie bezwzględnej orientacji obiektu w przestrzeni, pozycja w przestrzeni to trochę inna para kaloszy. Pozdr

    Racja, głupi błąd ;)

  • #8 01 Mar 2017 01:28
    rb401
    Poziom 29  

    Kwarcu napisał:
    Wrzucam w załączniku poprawione binarki, możecie sprawdzić czy to coś poprawiło.


    O tak. Lag zniknął jak ręką odjął.

    Jest tylko jakiś inny drobiazg, nie związany z tematem IMU.
    W Twojej skompilowanej binarce w głównym oknie, coś jakby "rozrabiało" z kontrolkami.
    Muszę akurat wybrać COMa drugiego z trzech obecnych w comboboxie a gdzieś po ułamku sekundy, "ktoś" zmienia mi wybór na pierwszy com z listy. Widać też że rozwinięta lista comboxa miga w tempie ok. sekundowym, jakby była okresowo odświeżana czy coś I robi się z tego "zręcznościówka" bo muszę z klawiatury sterować comboboxem a myszką błyskawicznie nacisnąć Connect, póki jest wybrany ten com co chcę, bo samą myszką się czasowo nie wyrabiam. Jak już Connect odpalę to combobox działa całkowicie normalnie.

    A co najdziwniejsze, to nie mam pojęcia co to jest, bo jak kompiluje Twoje źródła u siebie to z tym comboboxem jest cały czas wszystko w porządku.


    Kwarcu napisał:
    ale te wartości musiałbyś sam sobie wyznaczyć dla twojego czujnika. Ja korzystałem z tego: Link


    Nie wiem czy kojarzysz. Na nówce STM32L476-DISCOVERY było takie fabryczne demko, w tym kompas.

    Zajrzałem do źródeł i widzę że tam kalibracja magnetometru jest bardzo prosto zrobiona. Tak że chyba nie trzeba żmudnie bawić się tym pudełkiem i zewnętrznym programem i zaszywaniem macierzy w źródła, bo można by wyliczenie współczynników kalibracyjnych dorobić małym wysiłkiem do aplikacji. A myślę że jakość kalibracji z tego STMowego dema nie będzie jakaś drastycznie gorsza. Aplikacja kompasu po tej kalibracji jak dla mnie, to pokazywała bardzo sensowne wyniki. Pod warunkiem oczywiście, że odpowiednio się będzie poruszać płytką przy kalibracji i w miarę długo (by załapać ekstrema na wszystkich osiach).


    Wyznaczenie współczynników kalibracyjnych wg dema STM:

    Kod: c
    Zaloguj się, aby zobaczyć kod



    Wyliczenie skorygowanego pomiaru z magnetometru w funkcji kompasu:

    Kod: c
    Zaloguj się, aby zobaczyć kod

  • #9 01 Mar 2017 11:27
    adamusx
    Poziom 27  

    Witam,

    Fajny projekt edukacyjny, duży plus za czytelny kod, szczególnie na STMa!
    (porównując do dużej części innych projektów, gdzie naładowane są biblioteki typu HAL/SPL, panuje ogólny chaos i ten sam kod jest z 5x dłuższy i zagmatwany;)

    Jaką uzyskujesz stabilność odczytów kątów (roll/pitch) gdy płytka leży nieruchomo?

  • #10 01 Mar 2017 13:24
    Kwarcu
    Poziom 11  

    rb401 napisał:
    Jest tylko jakiś inny drobiazg, nie związany z tematem IMU.
    W Twojej skompilowanej binarce w głównym oknie, coś jakby "rozrabiało" z kontrolkami.
    Muszę akurat wybrać COMa drugiego z trzech obecnych w comboboxie a gdzieś po ułamku sekundy, "ktoś" zmienia mi wybór na pierwszy com z listy. Widać też że rozwinięta lista comboxa miga w tempie ok. sekundowym, jakby była okresowo odświeżana czy coś I robi się z tego "zręcznościówka" bo muszę z klawiatury sterować comboboxem a myszką błyskawicznie nacisnąć Connect, póki jest wybrany ten com co chcę, bo samą myszką się czasowo nie wyrabiam. Jak już Connect odpalę to combobox działa całkowicie normalnie.

    A co najdziwniejsze, to nie mam pojęcia co to jest, bo jak kompiluje Twoje źródła u siebie to z tym comboboxem jest cały czas wszystko w porządku.


    Hehe, pewnie dlatego, że po drodze kombinowałem jeszcze coś z odświeżaniem listy portów i ostatnie binarki wrzuciłem właśnie z tym bugiem. W źródłach w #1 poście tego niema. Ogólnie to już nie mam czasu na poprawianie tego projektu, dlatego udostępniłem źródła żeby ktoś sobie coś dopisał/poprawił pod siebie.

    rb401 napisał:
    Nie wiem czy kojarzysz. Na nówce STM32L476-DISCOVERY było takie fabryczne demko, w tym kompas.

    Tak, zaglądałem. Ale jak chciałem mieć tą "pełną" kalibrację ;)

    adamusx napisał:
    Jaką uzyskujesz stabilność odczytów kątów (roll/pitch) gdy płytka leży nieruchomo?

    Roll i pitch mają szybkie wahania ok. ±0.1° natomiast yaw pomału "pływa" (okres jakieś 4s) ok. ±1°. Te wahania zależą od wartości beta w filtrze madgwicka - ja ustawiłem na 0.041 (najmniejszy błąd dynamiczny i statyczny). Przy takiej wartości kostka wolno łapie orientację bezwzględną z warunków początkowych ale czytałem, że ludzie robią tą betę zmienną (na początku duża a potem zmniejszają) własnie po to aby po starcie szybko układ złapał kąty.

 Szukaj w ofercie
Zamknij 
Wyszukaj w ofercie 200 tys. produktów TME