Toto je starší verze dokumentu!
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 DT0076.
Cieľom tohto projektu je:
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).
K realizácii projektu bola použitá vývojová deska 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.
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.
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.).
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; }
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 DT0076 aj podľa funkcií MATLABU prebieha v pravotočivom súradnicovom systéme.
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
).
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(); } ... }
Derotačná matica je inverzná alebo transponovaná rotačná matica. Rotačné matice pre rotáciu okolo jednotlivých osí sú 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). Rovanko, 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 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)Výsledkom riešenia projektu je meranie pozdĺžneho a priečneho náklonu. Kompenzácia dát implementovaním derotácie dátového vektoru zrýchlenia