Tropjosí gyroskopický modul L3G4200D
Zo stránky SensorWiki
Autori: | Ján Benedek |
Martin Cintula | |
Michal Gáher | |
Študijný odbor: | Aplikovaná mechatronika |
Ročník: | 2. Ing. (2014) |
Zadanie
Naším zadaním bolo implementovať senzor L3G4200D - trojosí gyroskopický modul s mikropočítačom a získať z neho užitočné údaje. Gyroskop detekuje uhlovú rýchlosť natočenia vo všetkých troch osiach.
Senzor podporuje komunikáciu cez SPI a takisto aj cez I2C zbernicu. Pre náš projekt sme použili jednoduchšiu I2C zbernicu.
Následne sme vyčítané dáta mali zobraziť na PC vo virtualizačnom programe. Na tento účel poslúžil maltab.
Senzor je namontovaný na doske a predáva sa to ako celok. Na doske sa nachádzajú nejake obvody a čipy. Výsledok je, že nám stačí priviesť napájanie a I2C komunikáciu s pull up rezistormi a celé to potom funguje.
Literatúra:
- Zoznam použitej literatúry, vrátane katalógových údajov (datasheet), internetových odkazov a pod.
Zadanie
Senzor nám ponúka SPI a I2C zbernicu. Pre jednoduchosť sme použili I2C zbernicu. Chip podporuje protokol I2C, fast mode a takisto aj normal mode. Nemá žiadne špeciálne protokoli, preto je možné pre nás použiť I2C knižnicu od pána Flueriho.I2C
Casový diagram pre I2C:
![](/sensorwiki/images/8/86/Timef.jpg)
Parametre senzora:
- tri možnosti senzitivity
- I2C/SPI zbernice
- 16 bitový výstup dát
- 8 bitový výstup teploty
- Integrovaný dolno a horno priepustný filter
- Power down and sleep mode
Pre našu aplikáciu nám nebude potreba snímania teploty. Pracovisko má stabilnú teplotu. Takisto nebudeme používať žiaden filter a využijeme obyčajný operation mód. Senzitivita je nastaviteľná a bude možné ju meniť.
Senzor je už z výroby nakalibrovaný, avšak pre našu aplikáciu v matlabe budeme musieť urobiť našu vlastnú kalibráciu. Na určenie kalibračných konštánt potrebujeme systém ktorý nám umožní senzor natáčať vo všetkých troch osiach v rámci 180 stupňov. Jednoduchý servo motor nám bude stačiť. Tu využijeme PWM moduláciu na riadenie servo motora (parallax servo motor).
![](/sensorwiki/images/8/82/Serv.jpg)
Následne získane kalibračné hodnoty a uhlové rýchlosti nám pomožu na určenie natočenia/uhla. Tu použijeme skript v matlabe ktorý nám bude integrovať v reálnom čase. Rovnice:
![](/sensorwiki/images/f/fd/Integ.jpg)
Piny na čipe:
My využijeme iba 5 z možných 8. Dva slúžia na napájanie, dva na I2C komunikáciu a posledný pin SD0 použijeme iba na zmenu adresy. Urobili sme to tak lebo sme to videli v iných projektoch, ale čip by fungoval aj tak, len by sme museli v kóde zmeniť adresu.
Popis riešenia
Tento projekt rozložím do dvoch častí:
- Zapojenie na vývojovej doske ACROB
- Vizualizácia v matlabe
Zapojenie čipu k mirkoprocesoru.
Ako som vyššie spomenul, pin SD0 je pripojený na Vcc, a to zmení adresu čipu. Čip funguje aj bez SD0 pripojenia na Vcc ale vtedy treba zmeniť adresu v kóde.
Na obrázku je jasne vidieť ako máme pripojené 2 periférie. Servo motor nám slúži iba na kalibráciu. Senzor komunikuje cez I2C zbernicu a je potrebné k SDA a SCL pripojiť pull up rezistory aby komunikácia fungovala správne.
Senzor je na začiatku potrebné nekonfigurovať. Nachádza sa tu 5 Control registerov, ktoré je potrebné nakonfigurovať podľa požiadaviek na čip.
- CTRL_REG1 nastavuje zapnutie osí, a celkove zapnutie čipu
- CTRL_REG2 nastavuje filtre v čipe (my nepoužívame)
- CTRL_REG3 nastavuje prerušenia (tiež nepoužívame)
- CTRL_REG4 nastavuje citlivosť/typ zápisu dát (endian)/SPI enable
- CTRL_REG5 nastavuje prerušenia/filtre (nepoužívame)
Po úvodnej konfigurácií môžeme priamo pristupovať k dátam. Dáta sú uložené ako 16 bitové dáta v dvoch 8 bitových registroch. Tieto registre treba vyčítať a nasledne spojiť aby sme dostali finálnu hodnotu. Dáta su zapísane ako dvojkový doplnok, takže nám dávajú aj negatívne hodnoty. Registre idú za sebou, preto neni treba vkuse zapisovať adresu z ktorej chcem čítať, ale akonáhle je prečítaný prvý bajt, možme pokračovať na ďaľší a takto vieme vyčítať všetkých 6 8-bitových registrov a poskladať relevantné údaje.Adresa kde sa nachádzajú dáta začína 0xA8.
čip priamo posiela údaje o uhlovej rýchlosti v jendotkách [dps]= degrees per second (stupeň za sekundu) Ďalšie spracovanie dát nastáva už v matlabe. Dáta sú posielané po sériovej linke.
Vizualizácia
Celá vizualizácia sa deje v matlabe. Najprv sme museli nadviazať komunikáciu s vývojov doskou ACROB. Tak ako sme používali serial connection cez program Terminal, takisto použijeme tento typ spojenia pre matlab. Aby sme sa uistili, že komunikácia je nadviazaná správne, používame tzv. handshake. Je to zralizované tak, že ACROB posiela charakter 'a' po sériovej linke a je to v loope, takže pokým nenastane komunikácia s matlabom/iným sériovým kuminikátorom, kód sa nepohne ďalej. Keď matlab prečíta charakter 'a', pošle po sériovej linke späť charakter '2'. Týmto si sú obe zaraidenia isté, že komunikujú správne. Matlab k´d je tiež v loop pokým nepríjme charakter 'a'.
Rozhodol som sa, že uhlovú rýchlosť budem integrovať a ukazovať natočenie šípky. Šípka sa bude vlastne kopírovať pohyb celého senzora. Aby sme dostali skutočnú polohu/natočenie, potrebujeme zistiť kalibračné konštanty. Toto je spravené cez servo motor. Servo motor točí celý senzor od 0 stupňov až po 180 konštantnou rýchlosťou a počas tohoto pohybu neustále čítame dáta z čipu. Nakoľko je rýchlosť konštantná, dáta z čipu by mali dávať konštantnú uhlovú rýchlosť. Nakoľko vieme čas ako dlho trvá dosiahnutie 180 stupňnov, vieme aj o koľko stupňnov sa seznor otočil a vieme aj uhlovú rýchlosť, je jednoduché potom zistiť kalibračnú konštantu.
Tento proces sa deje pre všetky tri osi, a vrámci získania presných hodnôt, proces sa deje 5 krát a kalibračná konštanta je zpreimerovaná. Po kalibráci sa spustí vizuálne okno kde je nakreslená šípka. šípka može byť reprezentovaná v 2D ale aj v 3D. 2D sa ukázala byť viac prehladné. Šípka sa nakláňa tak ako senzor v 'x' a 'y' osi a natáča sa tak ako senzor v 'z' osi.
Ak sa zmení citlivosť senzora, je potrebné urobiť kalibráciu znova. Po získaní kalibračných konštánt, opätovná kalibrácia nie je potrebná.
V kóde sa nachádzajú dve spracovania dát, ak matlab pošle charakter 'G', potom senzor posila dáta normaálne, ak však matlab pošle charakter 'C', to znamená kalibrácia a kód riadi servo pohon a pošle dáta 50 krát. Pri charakteri 'G', dáta sú poslané raz treba neustále posielať charakter 'G' aby sme čítali dáta.
Algoritmus a program
Tu popíšem kód, ktorý bol nahratý do mikropočítača. Kód použitý v matlabe bude pripojený ako príloha, avšak je príliš rozsiahli na to aby som ho tu citoval.
Kód do mikroprocesoru bol robení v AVR studio 4, v jazyku C. Vizualizácia bola robená v matlabe v matlabovskom kóde.
Vývojový diagram na základe ktorého sme potom urobili kód:
![](/sensorwiki/images/5/5d/Vyvoj.jpg)
Máme main, v ktorý sa riadi vývojovým diagramom:
/* A nezabudnite zdroják hojne komentovať */
#include <avr/interrupt.h>
#include <stdlib.h>
#include <avr/io.h>
#include <util/delay.h>
#include <util/twi.h>
#include <stdio.h>
#include "serial.h"
//Definovanie citlivosti
#define L3G4200D_RANGE250 0x00
#define L3G4200D_RANGE500 0x10
#define L3G4200D_RANGE2000 0x30
void servo(int turn);
//pouzitie prikazov printf pre seriovu komunikaciu
FILE mystdout_Uart = FDEV_SETUP_STREAM(sendchar, NULL, _FDEV_SETUP_WRITE);
FILE mystdin_Uart = FDEV_SETUP_STREAM(NULL, recchar, _FDEV_SETUP_READ);
//inicializacne a pociatocne hodnoty a premenne
int16_t gxraw = 0;
int16_t gyraw = 0;
int16_t gzraw = 0;
int16_t range = L3G4200D_RANGE250;
int main(void)
{
//nastavenie portov pre servo
DDRB = 0x06; //Nastavenie pinov PB1 a PB2
PORTB = 0x01; //Nastavenie výstupu na log. 0
servo(100); //otocenie servo do vychodzej polohy
_delay_ms(100); //pockanie na otocenie
//inicializacia seriovej linky
inituart();
stdout = &mystdout_Uart;
stdin = &mystdin_Uart;
//inicializacia cipu
l3g4200d_init(&range);
_delay_ms(1500);
//handshake s matlabom
printf("a");
int x = 0;
while (x == 0)
{
x=getchar(); //prijem charakteru po seriovej linke
}
//loop kde sa caka na charekter z matlabu a posielanie dat do matlabu
while(1)
{
int mode=getchar(); //prijatie znaku z matlabu
switch (mode) //dve moznosti, bud kalibracia alebo posielanie dat
{
case 71: //ASCI kod pre G -> posielanie dat
l3g4200d_getrawdata(&gxraw, &gyraw, &gzraw); //priajtie dat -> pozri chip.c
printf("%d\n",gxraw); //poslanie po seriovej linke
printf("%d\n",gyraw);
printf("%d\n",gzraw);
break;
case 67: //ACSCI kod pre C -> kalibracia
servo(542); //otocenie serva o 180 stupnov
for (int i=1; i<=50; i++) //zber dat pocas natacanie (50 vzoriek)
{
l3g4200d_getrawdata(&gxraw, &gyraw, &gzraw); //priajtie dat -> pozri chip.c
printf("%d\n",gxraw); //poslanie po seriovej linke
printf("%d\n",gyraw);
printf("%d\n",gzraw);
}
_delay_ms(200);
servo(100); //natocenie serva spat
_delay_ms(200);
break;
}
}
return 0;
}
//servo funkcia na natacanie
void servo(int turn)
{
TCCR1A = 0b10100010; //mod: PWM, Phase Correct, 9-bit
TCCR1B = 0b00011011; //delicka 1:1024
ICR1 = 0x1387; //nastavenie topu/periody PWM - 20ms
TCNT1 = 0x00; //vynulovanie casovaca
OCR1A = turn; //nastavenie kedy ma togglovat
}
Potom máme vytvorený súbor chip.c kde sa nachádzajú dve funkcie, inicializácia čipu a čítanie dát z čipu:
#include <stdlib.h>
#include <avr/io.h>
#include <util/delay.h>
#include "chip.h"
#include "i2cmaster.h"
//funckia na vycitanie dat
void l3g4200d_getrawdata(int16_t *gxraw, int16_t *gyraw, int16_t *gzraw)
{
uint8_t i = 0;
uint8_t buff[6]; //buffer pre 8 bitove data
i2c_start(L3G4200D_ADDR | I2C_WRITE); //start I2C + poslanie adresy cipa a R/W
i2c_write(L3G4200D_OUT_X_L | (1 << 7)); //Definovanie adresy v cipe
i2c_rep_start(L3G4200D_ADDR | I2C_READ); //Vycitanie z cipu
//loop pre vycitanie vsetkych dat zo vsetkych osi
for(i=0; i<6; i++) {
if(i==6-1)
buff[i] = i2c_readNak();
else
buff[i] = i2c_readAck();
}
i2c_stop(); //stop I2C
//zkompletizovanie 16 bitovych dat
*gxraw = ((buff[1] << 8) | buff[0]);
*gyraw = ((buff[3] << 8) | buff[2]);
*gzraw = ((buff[5] << 8) | buff[4]);
}
//inicializacna funckia
void l3g4200d_init(int16_t *range) {
i2c_init(); //zapnutie I2C
_delay_us(10);
//enable chip
i2c_start(L3G4200D_ADDR | I2C_WRITE);
i2c_write(L3G4200D_CTRL_REG1);
i2c_write(0x0F);
i2c_stop();
//v podstate nepotrebne - len pre istotu
i2c_start(L3G4200D_ADDR | I2C_WRITE);
i2c_write(L3G4200D_CTRL_REG2);
i2c_write(0x00);
i2c_stop();
//v podstate nepotrebne - len pre istotu
i2c_start(L3G4200D_ADDR | I2C_WRITE);
i2c_write(L3G4200D_CTRL_REG3);
i2c_write(0x00);
i2c_stop();
//set range
i2c_start(L3G4200D_ADDR | I2C_WRITE);
i2c_write(L3G4200D_CTRL_REG4);
i2c_write(*range);
i2c_stop();
//v podstate nepotrebne - len pre istotu
i2c_start(L3G4200D_ADDR | I2C_WRITE);
i2c_write(L3G4200D_CTRL_REG5);
i2c_write(0x00);
i2c_stop();
}
Ďaľšie časti kódu sú už len I2C knižnica, a knižnica pre seriovú komunikáciu.
Pre ilustráciu uvediem matlabovský kód na spustenie sériovej linky s čipom a takisto aj pre vyčítanie dát z ACROB-u:
startSerial.m =>
function [s,flag] = setupSerial(comPort)
flag = 1;
s = serial(comPort);
set(s,'DataBits', 8 );
set(s,'StopBits', 1 );
set(s,'BaudRate', 9600 );
set(s,'Parity', 'none');
fopen(s);
a='b';
while (a ~='a')
a=fread(s,1,'uchar');
end
if (a=='a')
disp('serial read');
end
fprintf(s,'%d',2);
mbox = msgbox('Serial Communication setup.'); uiwait(mbox);
end
readGyro.m =>
function [gx, gy, gz] = readGyro(gyroConnection, isCalibration)
if(~isCalibration)
fprintf(gyroConnection.s,'G');
end
gx = fscanf(gyroConnection.s,'%d');
gy = fscanf(gyroConnection.s,'%d');
gz = fscanf(gyroConnection.s,'%d');
end
Táto funkcia je zavolaná len raz. V ďaľších skiptoch potom už len posielame charaktery 'G' alebo 'C' a čítame dáta, integrujeme a reprezentujeme to na šípke. Ostatné kódy su v prílohe.
Zdrojový kód: serial.h a main.c
Overenie
Ukážka Nezabudnite napísať čosi ako užívateľský návod. Z neho by malo byť jasné čo program robí, ako sa prejavuje a aké má užívateľské rozhranie (čo treba stlačiť, čo sa kde zobrazuje). Ak ste namerali nejaké signály, sem s nimi. Ak je výsledkom nejaký údaj na displeji, odfotografujte ho.
Kľúčové slová 'Category', ktoré sú na konci stránky nemeňte.