====== Výpočet náklonu z akcelerometru ======

===== 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 ======

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 ======
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