Hi there,
few days ago I sent an email to pololu’ staff regarding the output of minimu’ s semsor: in the description of this item there is written 12bit but, if you look the datasheet of sensors, they are all 16bit res. So there are few tricks we should set up in the library of lsm and gyro. Anyway, this code demostrates the true word resolution of the output of each device. It’ s pretty fast writing because there are some details there could be done better, but it works. ( arduino ide of course)
Main page
#include <Wire.h>
#define ATM 0x77
vectorINT acc, giro, mag;
void setup(){
Wire.begin();
Serial.begin(9600);
initDevices();
Serial.print("X:\tY:\tZ:\n");
}
void loop(){
vectorINT acc = ReadAcc();
Serial.print(acc.x);
Serial.print("\t");
Serial.print(acc.y);
Serial.print("\t");
Serial.print(acc.z);
Serial.print("\n");
}
Functions:
//Dispositivi TWI
#define IMUGYRO 0x6B
#define IMUACC 0x19
#define IMUMAG 0x1E
#define ATM 0x77
//Funzione di setup dispositivo
void lsmsetup(byte address, byte reg, byte setting){
Wire.beginTransmission(address);
Wire.send(reg);
Wire.send(setting);
Wire.endTransmission();
}
//Funzione di lettura dati da registro
int reading(int address, byte reg){
byte value;
Wire.beginTransmission(address);
Wire.send(reg);
Wire.endTransmission();
Wire.beginTransmission(address);
Wire.requestFrom(address, 1);
if(Wire.available()){
value=Wire.receive();
}
Wire.endTransmission();
return value;
}
//Funzione di lettura dati da registro (2 byte)
int reading2B(int address, byte reg){
int value;
Wire.beginTransmission(address);
Wire.send(reg);
Wire.endTransmission();
Wire.beginTransmission(address);
Wire.requestFrom(address, 2);
if(Wire.available()){
value=Wire.receive();
value <<= 8;
value |= Wire.receive();
}
Wire.endTransmission();
return value;
}
void initDevices(void){
Serial.print("Inizializzazione devices...");
//Setup giroscopio
// CTRL_REG1 address: 0x20 parameters: 0x0F
lsmsetup(IMUGYRO, 0x20, 0x0F);
//Setup accelerometro
// CTRL_REG1_A address: 0x20 parameters: 0x27
lsmsetup(IMUACC, 0x20, 0x27);
//Setup magnetometro
// MR_REG_M address: 0x02 parameters: 0x00
lsmsetup(IMUMAG, 0x02, 0x00);
Serial.print("ok\n");
}
//Funzione lettura dati accelerometro
vectorINT ReadAcc(){
vectorINT a;
byte xh= reading(IMUACC, 0x29); // accelerometer X
byte xl= reading(IMUACC, 0x28);
a.x = ((xh << 8) | xl);
if(a.x > 32767){
a.x -= 65535;
}
byte yh= reading(IMUACC, 0x2B); // accelerometer Y
byte yl= reading(IMUACC, 0x2A);
a.y = ((yh << 8) | yl);
if(a.y > 32767){
a.y -= 65535;
}
byte zh= reading(IMUACC, 0x2D); // accelerometer Z
byte zl= reading(IMUACC, 0x2C);
a.z = ((zh << 8) | zl);
if(a.z > 32767){
a.z -= 65535;
}
return a;
}
//Funzione lettura dati magnetometro
vectorINT ReadMag(){
vectorINT a;
byte xh, xl, yh, yl, zh, zl;
Wire.beginTransmission(IMUMAG);
//Registro MREGXH => X_HIGH
Wire.send(0x03);
Wire.endTransmission();
Wire.beginTransmission(IMUMAG);
Wire.requestFrom(IMUMAG, 6);
while(Wire.available() >= 6){
xh=Wire.receive();
xl=Wire.receive();
yh=Wire.receive();
yl=Wire.receive();
zh=Wire.receive();
zl=Wire.receive();
}
Wire.endTransmission();
//Unisce xlow e xhigh e corregge
a.x =(xh << 8 | xl);
if(a.x > 32767){
a.x -= 65535;
}
//Unisce ylow e yhigh e corregge
a.y =(yh << 8 | yl);
if(a.y > 32767){
a.y -= 65535;
}
//Unisce zlow e zhigh e corregge
a.z =(zh << 8 | zl);
if(a.z > 32767){
a.z -= 65535;
}
return a;
}
//Funzione lettura dati accelerometro
vectorINT ReadGiro(){
vectorINT a;
byte xh= reading(IMUGYRO, 0x29); // accelerometer X
byte xl= reading(IMUGYRO, 0x28);
a.x = ((xh << 8) | xl);
if(a.x > 32767){
a.x -= 65535;
}
byte yh= reading(IMUGYRO, 0x2B); // accelerometer Y
byte yl= reading(IMUGYRO, 0x2A);
a.y = ((yh << 8) | yl);
if(a.y > 32767){
a.y -= 65535;
}
byte zh= reading(IMUGYRO, 0x2D); // accelerometer Z
byte zl= reading(IMUGYRO, 0x2C);
a.z = ((zh << 8) | zl);
if(a.z > 32767){
a.z -= 65535;
}
return a;
}
The vector datatype is the standard math(float) vector composed by float x, y and z. The vectorINT is a typedef I’ ve introduced because the raw data is in int and pass it though float would have taken more memory. This is an exctact of my code, it’ s thought like a reference (for example instead of calling everytime the registers you can use the autoincrementation which make everything easier and cleaner). I’ ll attach a complete and ready-to-use library to read imu datas and filter with kalman method soon. Thank you