Zde můžete vidět rozdíly mezi vybranou verzí a aktuální verzí dané stránky.
Následující verze | Předchozí verze | ||
2018:accel-roll-pitch [2019/01/14 12:06] Ondrej Vörös vytvořeno |
2018:accel-roll-pitch [2019/01/14 23:58] (aktuální) Ondrej Vörös |
||
---|---|---|---|
Řádek 1: | Řádek 1: | ||
- | ====== Zadanie ====== | + | ====== Výpočet náklonu z akcelerometru ====== |
- | Na vývojové desce STM32F4DISCOVERY s akcelerometrem LIS302DL/LIS3DSH realizujte výpočet příčného a podélného náklonu (roll, pitch). Uvažujte libovolnou polohu akcelerometru vůči základně, tj. implementujte derotaci vstupního datového vektoru dle postupu v [[https://www.st.com/resource/en/design_tip/dm00358510.pdf|DT0076]]. | + | |
+ | ===== Zadanie ===== | ||
+ | Na vývojové desce STM32F4DISCOVERY s akcelerometrem LIS302DL/LIS3DSH realizujte výpočet příčného | ||
+ | a podélného náklonu (roll, pitch). Uvažujte libovolnou polohu akcelerometru vůči základně, | ||
+ | t.j. implementujte derotaci vstupního datového vektoru dle postupu | ||
+ | v [[https://www.st.com/resource/en/design_tip/dm00358510.pdf|DT0076]]. | ||
---- | ---- | ||
+ | |||
+ | ===== Úvod ===== | ||
+ | Cieľom tohto projektu je: | ||
+ | |||
+ | * realizovať výpočet priečneho a pozdĺžneho náklonu(angl. roll a pitch) | ||
+ | * kompenzovať náklon pomocou derotačnej matice pre ľubovoľnú polohu akcelerometru voči referenčnej rovine | ||
+ | |||
+ | Derotačná matica bude počítaná pomocou Matlabu, pretože obsahuje goniometrické funkcie. | ||
+ | Program bude možné ovládať cez UART konzolu (vložiť derotačnú maticu, kompenzovať výpočet náklonu). | ||
+ | |||
+ | {{https://www.novatel.com/assets/Web-Phase-2-2012/Solution-Pages/AttitudePlane.png }} | ||
+ | |||
+ | ===== Hardware ===== | ||
+ | K realizácii projektu bola použitá vývojová deska | ||
+ | [[https://www.st.com/en/evaluation-tools/stm32f4discovery.html|STM32F4DISCOVERY]] | ||
+ | od firmy STMicroelectronics, súčasťou ktorej je 3-osový MEMS akcelerometer LIS302DL/LIS3DSH. | ||
+ | Vývojová doska taktiež obsahuje ST-Link/V2 programátor/debugger. Je založená na jadre ARM Cortex-M4, ktorý je | ||
+ | taktovaný 8MHz kryštálom, má 1MB Flash pamäť a 192kB RAM. K umožneniu ovládania programu z počítača cez konzolu | ||
+ | pomocou sériovej linky bol použitý UART-USB prevodník s čipom CP2102. | ||
+ | |||
+ | ===== Software ===== | ||
+ | Ako vývojové prostredie bolo použitý EmBitz. Základné části programu s nastavením periférií boli vytvorené | ||
+ | v programe STM32CubeMX, nastavené bolo sériové rozhranie UART s DMA kontrolérom a časovač Timer2. | ||
+ | K ovládaniu akcelerometru bol použitý BSP (Board Support Package) knižnice vývojovej dosky. | ||
+ | |||
+ | ==== Výpočet náklonu ==== | ||
+ | Pre výpočet pozdĺžneho a priečneho náklonu roll a pitch bola vytvorená funkcia ''get_degree_tilt''. Pri pomalom otáčaní | ||
+ | pôsobí na akcelerometer hlavne gravitačná sila, zrýchlenie v jednotlivých osách akcelerometru je dané gravitačným | ||
+ | zrýchlením. Skusmým otáčaním dosky s akcelerometrom boli zistené maximálne hodnoty zrýchlenia v jednotlivých osách, | ||
+ | pre účely merania náklonu bolo urobené obmedzenie hodnôt zrýchlenia zhora a nasleduje prepočet na uhly 0° až 90° | ||
+ | (-90° až 0° resp.). | ||
+ | |||
+ | <code c> | ||
+ | float get_degree_tilt(float val, int16_t max_val) | ||
+ | { | ||
+ | float deg_tilt; | ||
+ | /* constrain values */ | ||
+ | if(val > max_val) { | ||
+ | val = max_val; | ||
+ | } | ||
+ | else if(val < -max_val) { | ||
+ | val = -max_val; | ||
+ | } | ||
+ | |||
+ | /*map values to range -90° - 90° */ | ||
+ | deg_tilt = (90*val) / max_val; | ||
+ | |||
+ | return deg_tilt; | ||
+ | } | ||
+ | </code> | ||
+ | |||
+ | Na nasledujúcom obrázku je zobrazený pravotočivý súradnicový systém na vývojovej doske. Podľa hodnôt získaných | ||
+ | z akcelerometru má ten orientáciu osí podľa ľavotočivého súradnicového systému, preto pri výpočte náklonu sa | ||
+ | hodnotám zrýchlenia v ose x otáča znamienko (otáčame x-ovú os), keďže výpočet derotačnej matice podľa | ||
+ | [[https://www.st.com/resource/en/design_tip/dm00358510.pdf|DT0076]] aj podľa | ||
+ | [[https://www.mathworks.com/help/phased/ref/rotx.html| funkcií MATLABU]] prebieha v pravotočivom súradnicovom | ||
+ | systéme. | ||
+ | |||
+ | {{ 2018:accel-roll-pitch:sur_system-na-doske.png?400 }} | ||
+ | |||
+ | Priečny náklon je definovaný ako rotácia okolo osi x (použité hodnoty zrýchlenia v osi y) a pozdĺžny náklon | ||
+ | ako rotácia okolo osi y (použité hodnoty zrýchlenia v osi x). Náklon v jednotlivých smeroch je indikovaný aj | ||
+ | pomocou štyroch LED diód umiestených na vývojovej doske do kríža po stranách akceleremetru (funkcia ''tilt_light''). | ||
+ | |||
+ | <code c> | ||
+ | while (1) | ||
+ | { | ||
+ | if ((HAL_GetTick() - tick) >= per_meas) { // period of measurement | ||
+ | BSP_ACCELERO_GetXYZ(buffer); | ||
+ | accX = buffer[0]; | ||
+ | accY = buffer[1]; | ||
+ | accZ = buffer[2]; | ||
+ | |||
+ | tilt_light(accX, accY); | ||
+ | |||
+ | pitch = get_degree_tilt(-accX, max_xval); //pitch- acceleration around axis y --> accX , -accX --turning around axis X to get right-rotated coordinate system | ||
+ | roll = get_degree_tilt(accY, max_yval); // get roll- acceleration around axis x --> accY | ||
+ | |||
+ | printf("roll,pitch:\t%2.1f%c\t%2.1f%c\n", roll, deg_sign, pitch, deg_sign); | ||
+ | |||
+ | tick = HAL_GetTick(); | ||
+ | } | ||
+ | ... | ||
+ | } | ||
+ | </code> | ||
+ | |||
+ | ==== Výpočet derotačnej matice a kompenzácia náklonu ==== | ||
+ | Derotačná matica je inverzná alebo transponovaná rotačná matica. Rotačné matice pre rotáciu okolo jednotlivých osí | ||
+ | sú [[https://www.mathworks.com/help/phased/ref/rotx.html|definované]] takto: | ||
+ | |||
+ | Rx = [1 0 0; 0 cos(roll) -sin(roll); 0 sin(roll) cos(roll)]; % rotacna matica okolo osi x- counterclockwise | ||
+ | Ry = [cos(pitch) 0 sin(pitch); 0 1 0; -sin(pitch) 0 cos(pitch)]; % rotacna matica okolo osi y- counterclockwise | ||
+ | Rz = [cos(yaw) -sin(yaw) 0; sin(yaw) cos(yaw) 0; 0 0 1]; % rotacna matica okolo osi z- counterclockwise | ||
+ | |||
+ | Keďže pomocou akcelerometru nie je možné počítať uhol kurzu (otočenie okolo osi z- yaw), rotačnú maticu ''Rz'' | ||
+ | nevyužijeme. Ako derotačnú maticu sme použili ''M = Rx*Ry'' (bola určená experimentálne, aby vektor zrýchlenia | ||
+ | [1000 0 0] pri rotácii o 90° pozdĺžneho náklonu-pitch sa otočil do polohy [0 0 1000]- nasmerovanie osi z | ||
+ | do smeru gravitačného zrýchlenia). Rovnako, pri použití rotačnej matice M sa vektor zrýchlenia [0 -1000 0] | ||
+ | pri rotácii o 90° priečneho náklonu-roll otočí do polohy [0 0 1000]. | ||
+ | |||
+ | Derotovanie dátového vektora (v našom prípade vektoru zrýchlenia) prebieha vynásobením vektoru s derotačnou maticou. | ||
+ | Následne možno z derotovaného vektoru znovu vypočítať uhly náklonu. | ||
+ | |||
+ | accXr = M(1,1)*accX + M(1,2)*accY + M(1,3)*accZ | ||
+ | accYr = M(2,1)*accX + M(2,2)*accY + M(2,3)*accZ | ||
+ | accZr = M(3,1)*accX + M(3,2)*accY + M(3,3)*accZ | ||
+ | |||
+ | ==== Štruktúra programu ==== | ||
+ | Štruktúra programu je nasledovná, v nekonečnej smyčke prebieha výpočet a zobrazovanie náklonu v časovom intervale | ||
+ | jedna sekunda a tiež spracovanie znakov z kruhového zásobníka prijímaných sériovým rozhraním UART. K ovládaniu | ||
+ | programu cez sériové rozhranie slúžia dva príkazy: | ||
+ | * ''COMPENSATE'' - spustenie kompenzovaného výpočtu náklonu (30 meraní s periódou 1 sekunda) | ||
+ | * ''MATRIX m11 m12 m13 m21 m22 m23 m31 m32 m33'' - zadanie derotačnej matice (zadanie prvkov matice po riadkoch) | ||
+ | |||
+ | ----- | ||
+ | |||
====== Záver ====== | ====== Záver ====== | ||
+ | Výsledkom riešenia projektu je meranie pozdĺžneho a priečneho náklonu. Tiež bola implementovaná derotácia dátového | ||
+ | vektoru zrýchlenia pre ľubovoľnú polohu akcelerometru voči referenčnej rovine. Kompenzácia dát derotáciou | ||
+ | dátového vektoru zrýchlenia podľa hodnôt náklonu akcelerometru vo zvolenej polohe nie je žiaľ presná (nepresnosť | ||
+ | kolíše v závislosti na zvolenej polohe akcelerometru) a nepodarilo sa mi prísť (kvôli nedostatku času) na to, | ||
+ | kde môže byť chyba. | ||
+ | [[https://drive.google.com/open?id=16A5mXGF80x7sn3EjSTIg6NrUqWS896O5|zdrojové súbory]] | ||
+ | [[https://drive.google.com/open?id=17qJpzuhK15dT8O1v8BeUGPA5j-lbc9JG|videoukážka]] | ||
---- | ---- | ||
- | ====== Zdroje ====== | ||
- | DT0076- Compensating for accelerometer installation error: zeroing pitch and roll for a reference orientation: [[https://www.st.com/resource/en/design_tip/dm00358510.pdf]] | ||
- | [[https://www.zive.cz/clanky/pojdme-programovat-elektroniku-jak-vlastne-funguje-akcelerometr-a-gyroskop-nejen-ve-vasem-telefonu/sc-3-a-194858/default.aspx#articleStart]] | + | ====== Zdroje ====== |
- | + | https://www.st.com/resource/en/design_tip/dm00358510.pdf\\ | |
- | Rotation matrix for rotations around x-axis: https://www.mathworks.com/help/phased/ref/rotx.html | + | https://www.zive.cz/clanky/pojdme-programovat-elektroniku-jak-vlastne-funguje-akcelerometr-a-gyroskop-nejen-ve-vasem-telefonu/sc-3-a-194858/default.aspx#articleStart\\ |
+ | https://www.mathworks.com/help/phased/ref/rotx.html |