Goedkope 10DOF met I2C aansluiting
IMU met 10DOF met I2C aansluiting
IMU is de afkorting van "Inertial Measurement Unit", oftewel een meetsysteem dat zich baseert op inertie. Bedoeling is om de stand van een lichaam in de ruimte te meten tov een vaste referentie (in ons geval de aarde). Typische toepassingen zijn de balansrobot (welke hellingshoek heeft de robot) of een (model) vliegtuig, helikopter, quadcopter...
De combinatie van de verschillende soorten sensoren kan uiteindelijk op een vrij acurate wijze de stand van een lichaam tov de aarde meten. We spreken hier dan wel over de hoeken die dit lichaam inneemt tov onze referentie. Dankzij de luchtdruksensor kan er ook nog wel een zinvolle meting gedaan worden van de hoogte, maar dan wel met een resolutie van ongeveer 50 cm. Indien we nog meer informatie nodig hebben, kan een bijkomende GPS meting de absolute breedte en lengtegraad meten, maar ook met een resolutie van enkele meters.
Hardware : hoe de print aansluiten
Ik kon nergens een schema vinden van de print, maar omdat alle chips 3.3Volt zijn, heb ik de I2C van mijn discovery bord (3V)gebruikt om het een en het ander uit te testen. De pull-ups van 4.7k zijn voorzien op de print. De I2C clock loopt op 400 kHz. Ik kon dadelijk verbinding maken met alle 4 chips, maar op de scope zag je dat de stijgende flanken van SCL en SDA behoorlijk afgevlakt worden. Naast de I2C aansluitingen zijn er ook nog enkele interrupts van de verschillende chips naar buiten gebracht (vb BMP085 "End of conversion").
ADXL345
Dit is een welgekende 3-assige versnellingssensor. De versnelling van elke as wordt met maximaal 13-bit weergegeven. Voor de 3 assen moet je dus 6 bytes gaan uitlezen. Je kan verschillende gevoeligheden instellen, maar zelfs de hoogste gevoeligheid geeft maar een resolutie van 284 bits/g. Standaard worden de metingen elke 10 ms herhaald (Data rate = 100 Hz). Je kan de bandbreedte desgewenst verhogen tot 1600 Hz. Bij power-on staat de sensor in Standby. Om de metingen te starten moet je bit D3 zetten in register 0x2D. Ook standaard ingesteld is "10 bit resolutie" .Dit betekent dat de maximale meetwaarde van -511 tot +512 loopt. Je kan echter ook een hogere resolutie (13 bit) instellen : register 0x31. Dit bleek noodzakelijk omdat de Z-as een zeer grote offset heeft !Ik heb volgende registers ingesteld bij initialisatie :
I2C_Data[0]= 0x2D;I2C_Data[1]= 0x08; //Waarde 0x08 in register 0x2D, start meten met 100 Hz Refresh I2C_Master_BufferWrite(I2C2,I2C_Data,2,Polling,ADXL345); I2C_Data[0]= 0x31;I2C_Data[1]= 0x0B; //Waarde 0x0B in register 0x31, 13 bit resolutie ! I2C_Master_BufferWrite(I2C2,I2C_Data,2,Polling,ADXL345); //full res nodig vanwege grote offset Z-as
Zeer belangrijk is om de offset (de waarde bij 0 g)van elke as te meten, en dit dan te corrigeren in de software. Deze offsets waren behoorlijk groot, voor de Z-as zelfs 3 g ! In 2de instantie kan je ook nog de gevoeligheid corrigeren. Bij mijn sensor had de X-as een gevoeligheid van 292 bit/g, y-as 284 bit/g en de z-as 264 bit/g.
x_acc= I2C_Data[0]+(I2C_Data[1]<<8)+197; //offset X-as y_acc= I2C_Data[2]+(I2C_Data[3]<<8)+140; //offset Y-as z_acc= I2C_Data[4]+(I2C_Data[5]<<8)-818; //offset Z-as
De offset kan je meten door van elke as in twee standen de valversnelling te meten (180° verdraaien). De som van de 2 uitlezingen, gedeeld door 2 is dan de offset. Als je de offset juist hebt ingegeven, moet voor elke as de valversnelling in de 2 richtingen ongeveer gelijk van grootte, maar tegengesteld van teken zijn. Je kan ook je een meting doen als de sensor in "vrije val" is. Op dat moment meten al de 3 assen versnelling 0. Ik heb dit gedaan door mijn robot omhoog te gooien en terug op te vangen. Met de Xbee stuurde ik alle 100ms de meetwaarden van de 3 assen door.
In 2de instantie kan je ook nog de gevoeligheid van elke as corrigeren. Bij mijn sensor had de X-as 292 bit/g, de Y-as 284 bit/g en de Z-as 264 bit/g. Dit kan je eenvoudig meten door met de sensor in de 3 standen de valversnelling te meten (telkens uit richten zodat je de hoogste waarde meet). Nu kan je met de functie Atan2(x_axx,y_acc)de hoek in radialen berekenen van de actuele versnelling tov het XY vlak van de sensor. Dit kan je ook doen voor het XZ en het YZ vlak. Meestal worden deze hoeken gebruikt om te meten welke stand je robot / quadcopter / vliegtuig inneemt tov de aarde. Helaas worden deze metingen ook beinvloed door andere versnellingen van het object, dus een acc sensor alleen is niet voldoende. Ook kan je hiermee nooit de "heading" (naar welke windstreek wijst mijn object) bepalen, omdat de aardversnelling altijd loodrecht naar beneden wijst. De ruis van de sensor is ongeveer +/- 2bit. Over 100 samples vond ik een standaardafwijking van 0.96 voor de X en Z-as, de Y-as had 0.82. Mits oversampling kan je dus nog wat nauwkeuriger meten, maar met een lagere bandbreedte.
HMC5883L
Dit is een 3-assige magneetveld sensor van Honeywell. Hij heeft een maximaal meetbereik van -8 Gauss tot +8 Gauss, met een resolutie van 5 milliGauss. Het aardmagnetisch veld heeft een grootorde van 0.5 Gauss, dus deze sensor is perfect geschikt om als een 3-D kompas te gebruiken. Volgens de spec. is een nauwkeurigheid van +/- 2° te bereiken. De maximale samplerate is 75 Hz. Intern kan er ook gekozen worden voor "oversampling". Standaard wordt er een gemiddelde van 8 metingen in de registers gezet. De chip kan continu meten, of discontinu. Het starten van de metingen en het uitlezen van de registers is vrij eenvoudig. Ook hier is een kalibratie weer noodzakelijk om een zinvolle meting te doen van het aanwezige magneetveld. De offset van elke as kan je meten door het aardmagnetisch veld te meten in verschillende ruimtelijke posities van de print. Belangrijk hierbij is dat je de print op een vast plaats monteert op je robot. Reden : alle "magnetische" onderdelen van je robot veroorzaken een afwijking van het magnetisch veld. Als je kalibreert kan je deze afwijkingen mee "kalibreren". Initialisatie voor continue te meten :
I2C_Data[0]= 0x00;I2C_Data[1]= 0x70;I2C_Data[2]= 0x20;I2C_Data[3]= 0x0; //Schrijf 0x70 in reg 0 I2C_Master_BufferWrite(I2C2,I2C_Data,4,Polling,HMC588); //Schrijf 0x20 in reg 1, 0x0 in reg 2
Daarna kan je elke 15 ms de 6 data bytes uitlezen :
I2C_Data[0]=0x03; I2C_Master_BufferWrite(I2C2,I2C_Data,1,Polling,HMC588); //vanaf register 0x03 6 bytes uitlezen. I2C_Master_BufferRead(I2C2,I2C_Data,6,Polling,HMC588); //HMC x_mag= (I2C_Data[0]<<8)+I2C_Data[1]-x_hmc_offset; //offset kompenseren y_mag= (I2C_Data[2]<<8)+I2C_Data[3]-y_hmc_offset; z_mag= (I2C_Data[4]<<8)+I2C_Data[5]-z_hmc_offset;
L3G4200D
Een 3-assige gyro van ST met 16-bit resolutie. In de hoogste gevoeligheid kan deze gyro een maximale draaisnelheid van 250 °/sek meten, maar je kan ook een hoger meetbereik kiezen. Er bevindt zich ook een temperatuursensor op de chip, zodat je eventueel een temperatuurafhankelijke offset kan toepassen. Deze gyro kan gebruikt worden als gyro-kompas, maar een nauwkeurige drift-compensatie is dan noodzakelijk. Het is mogelijk om de drift na kalibratie te beperken tot 1°/min. Ikzelf doe steeds een kalibratie na het inschakelen van de voedingsspanning. Het kalibratie algoritme is zeer eenvoudig :
- Schakel de gyro in en zorg ervoor dat je robot gedurende de kalibratie niet beweegt.
- Lees elke 25 ms een nieuwe waarde van de gyro en sommeer deze
- Na 2500 ms (=100 samples)kan je deze som dan delen door 100 (integer deling). Bereken ook de rest van deze deling (div).
- Dit wordt dan de correctie die je bij elke volgende meting mee gaat sommeren.
- Om de 100 metingen kan je dan nog eens de "rest" van deze deling mee sommeren.
Daarnaast is het ook noodzakelijk dat je de "gain" ook nog eens kalibreert per as. Dit is natuurlijk ook een functie van de sample rate (ik gebruik 25ms). Immers, de drift kalibratie heeft als doel het nulpunt van de sensor zo juist mogelijk te bepalen, maar de gevoeligheid kan ook best nagemeten worden en gecorrigeerd. In het meetbereik van max 250°/sek bedraagt de resolutie 8.75 digit/milligraad/seconde. Dit betekent dat bij een draaisnelheid van 10°/sek de output 10000/8.75 = 1143 digits bedraagt. Omdat deze waarde 40 keer per seconde geintegreerd wordt, wordt de som dan 1143*40 = 45714 digits. Na 1 seconde is de draaihoek natuurlijk 10°, dus als ik de som deel door 4571 krijg ik de hoek in graden. De exacte gevoeligheid kan echter ook gemeten worden : Ik laat hiertoe mijn robot een aantal gekende omwentelingen maken met een draaisnelheid die kleiner is dan 250°/sek. Daarna vergelijk je de geintegreerde hoek met de werkelijke hoek. Deze verhouding is dan de gemeten gevoeligheid. Vb voor de Z-as moet ik deze som delen door 4430 om een hoek in graden te bekomen.
void Task_L3G4200D(void){
if (L3G_timer>=25){ //Elke 25 ms de metingen van de 3-assen uitlezen
L3G_timer=0;
I2C_Data[0]=0xA6;//Adress pointer for auto increment
I2C_Master_BufferWrite(I2C2,I2C_Data,1,Polling,L3G42);
I2C_Master_BufferRead(I2C2,I2C_Data,8,Polling,L3G42); //8 bytes lezen : statusbytes + 3*16bit meetwaarden
t_l3g=I2C_Data[0];
x_gier=I2C_Data[2]+(I2C_Data[3]<<8);
y_gier=I2C_Data[4]+(I2C_Data[5]<<8);
z_gier=I2C_Data[6]+(I2C_Data[7]<<8);
static uint8_t teller;
static int16_t x_offset,y_offset,z_offset,x_mod,y_mod,z_mod;
x_som=x_som+x_gier-x_offset;x_hoek=x_som/4511;//integratie om hoek te bekomen X-as
y_som=y_som+y_gier-y_offset;y_hoek=y_som/4367;// Y-as
z_som=z_som+z_gier-z_offset;z_hoek=z_som/4430 ;//Z-as
if(cal_L3G==2){ x_som=0;y_som=0;z_som=0;
x_offset=0;y_offset=0;z_offset=0;
x_mod=0;y_mod=0;z_mod=0;teller=0;cal_L3G=1;} //resetten nulpunt
if(teller==128){
if(cal_L3G==1){ //calibratie gyro door drift over 2.5 sekonde te meten
x_offset=x_som/128;
y_offset=y_som/128;
z_offset=z_som/128;
x_mod=x_som%128;
y_mod=y_som%128;
z_mod=z_som%128;
cal_L3G=0;x_som=0;y_som=0;z_som=0;
}
x_som=x_som+x_mod;y_som=y_som+y_mod;z_som=z_som+z_mod;
teller=0;
}
teller++;
}