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:

• 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).

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). 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 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. 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. zdrojové súbory videoukážka

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
https://www.mathworks.com/help/phased/ref/rotx.html