Go Down

Topic: Check my pololu mini 9 IMU code (Read 919 times) previous topic - next topic

sirbow2

Apr 17, 2012, 06:07 pm Last Edit: Apr 17, 2012, 11:07 pm by sirbow2 Reason: 1
ive been trying to create a libraryless arduino program to interface with my pololu miniIMU9(http://www.pololu.com/catalog/product/1265) in an attempt to learn more about spi/IMUs. it has a L3G4200D(gyro) datasheet and LSM303DLM(acc+mag). can someone look over it? thanks!

Code: [Select]
#include <Wire.h>

#define ACC_ADR 0b1100000 //(0x30 >> 1), 0x60
#define MAG_ADR 0b11110 //(0x3C >> 1), 0x1E
#define GYRO_ADR 0b1101001 //(0D2 >> 1), 0x69

#define ACC_EN 0x27 //enable ACC, Normal power mode, all axes enabled
#define GYRO_EN 0x0F //enable Gyro
#define MAG_EN 0x00 //Enable MAG, Continuous conversion mode

//ACC Registers:
#define ACC_CTRL_REG1 0x20
#define LSM303_OUT_X_L_A 0x28
int ACCx;
int ACCy;
int ACCz;
//Bias
int bACCx;
int bACCy;
int bACCz;

//Gyro Reg
#define L3G4200D_CTRL_REG1 0x20
#define L3G4200D_OUT_X_L 0x28
int GYROx;
int GYROy;
int GYROz;
//Bias
int bGYROx;
int bGYROy;
int bGYROz;

//MAG REGs
#define MAG_REG 0x02
#define LSM303_OUT_X_H_M 0x03
int MAGx;
int MAGy;
int MAGz;

//YAW
int xMAGMax;
int yMAGMax;
int zMAGMax;
int xMAGMin;
int yMAGMin;
int zMAGMin;
float xMAGMap;
float yMAGMap;
float zMAGMap;

//final values
int pitchAccel;
int pitchGyro;
int rollAccel;
int rollGyro;
float yawRaw=0;
float YawU;

void setup() {
 // put your setup code here, to run once:
 WriteReg(GYRO_ADR, L3G4200D_CTRL_REG1, GYRO_EN); //enable Gyro
 WriteReg(ACC_ADR, ACC_CTRL_REG1, ACC_EN); //enable ACC
 WriteReg(MAG_ADR, MAG_REG, MAG_EN); //enable MAG

 SetupAccel();
}

void loop() {
 // put your main code here, to run repeatedly:  
 ReadACC();
 ReadGYRO();
 ReadMAG();
 Calculations();
}

void WriteReg(byte ADR, byte reg, byte value)
{
 Wire.beginTransmission(ADR);
 Wire.write(reg);
 Wire.write(value);
 Wire.endTransmission();
}

void ReadMAG()
{
 Wire.beginTransmission(MAG_ADR);
 Wire.write(LSM303_OUT_X_H_M);
 Wire.endTransmission();
 Wire.requestFrom(MAG_ADR, 6);

 while (Wire.available() < 6);

 byte xhm = Wire.read();
 byte xlm = Wire.read();
 byte zhm = Wire.read();
 byte zlm = Wire.read();
 byte yhm = Wire.read();
 byte ylm = Wire.read();

 MAGx = (xhm << 8 | xlm);
 MAGy = (yhm << 8 | ylm);
 MAGz = (zhm << 8 | zlm);
}

void ReadACC()
{
 Wire.beginTransmission(ACC_ADR);
 // assert the MSB of the address to get the accelerometer
 // to do slave-transmit subaddress updating.
 Wire.write(LSM303_OUT_X_L_A | (1 << 7));
 Wire.endTransmission();
 Wire.requestFrom(ACC_ADR, 6);

 while (Wire.available() < 6);

 byte xla = Wire.read();
 byte xha = Wire.read();
 byte yla = Wire.read();
 byte yha = Wire.read();
 byte zla = Wire.read();
 byte zha = Wire.read();

 ACCx = (xha << 8 | xla) >> 4;
 ACCy = (yha << 8 | yla) >> 4;
 ACCz = (zha << 8 | zla) >> 4;
}

void ReadGYRO()
{
 Wire.beginTransmission(GYRO_ADR);
 // assert the MSB of the address to get the gyro
 // to do slave-transmit subaddress updating.
 Wire.write(L3G4200D_OUT_X_L | (1 << 7));
 Wire.endTransmission();
 Wire.requestFrom(GYRO_ADR, 6);

 while (Wire.available() < 6);

 byte xla = Wire.read();
 byte xha = Wire.read();
 byte yla = Wire.read();
 byte yha = Wire.read();
 byte zla = Wire.read();
 byte zha = Wire.read();

 GYROx = xha << 8 | xla;
 GYROy = yha << 8 | yla;
 GYROz = zha << 8 | zla;
}

void SetupAccel()
{
 //temporary total holders
 int tGYROx;
 int tGYROy;
 int tGYROz;

 int tACCx;
 int tACCy;
 int tACCz;

 delay(100); //wait for stable values
 for(int i = 0; i<50; i++) //get values fifty times
 {
   ReadACC();
   ReadMAG();
   ReadGYRO();

   tGYROx += GYROx; //total gyrox value += current reading
   tGYROy += GYROy;
   tGYROz += GYROz;

   tACCx += ACCx;
   tACCy += ACCy;
   tACCz += ACCz;
 }

 bGYROx = tGYROx / 50; //bias in gyro x = total gyro x/50
 bGYROy = tGYROy / 50;
 bGYROz = tGYROz / 50;

 bACCx = tACCx / 50;
 bACCy = tACCy / 50;
 bACCz = (tACCz / 50) - 256; //Don't compensate gravity away! We would all (float)!
}


void Calculations()
{
 //Calculate the pitch in degrees as measured by the accelerometers. Note that 8g into 11 bits gives 256 bits/g
 pitchAccel = atan2((ACCy - bACCy) / 256, (ACCz - bACCz) / 256) * 360.0 / (2*PI);
 //Calculate the pitch in degrees as measured by the gyros, angle = angle + gyro reading * time step.
 pitchGyro = pitchGyro + ((GYROx - bGYROx) / 70) * 0.02; //0.02 = gyro angle/sec
 //ditto for roll
 rollAccel = atan2((ACCx - bACCx) / 256, (ACCz - bACCz) / 256) * 360.0 / (2*PI);
 rollGyro = rollGyro - ((GYROy - bGYROy) / 70) * 0.02; //0.02 = gyro angle/sec

 //YAW!
 //this part is required to normalize the magnetic vector
 //get Min and Max Reading for MAGic Axis
 if (MAGx>xMAGMax) {xMAGMax = MAGx;}
 if (MAGy>yMAGMax) {yMAGMax = MAGy;}
 if (MAGz>zMAGMax) {zMAGMax = MAGz;}
 
 if (MAGx<xMAGMin) {xMAGMin = MAGx;}
 if (MAGy<yMAGMin) {yMAGMin = MAGy;}
 if (MAGz<zMAGMin) {zMAGMin = MAGz;}

 //Map the incoming Data from -1 to 1
 xMAGMap = float(map(MAGx, xMAGMin, xMAGMax, -30000, 30000))/30000.0;
 yMAGMap = float(map(MAGy, yMAGMin, yMAGMax, -30000, 30000))/30000.0;
 zMAGMap = float(map(MAGz, zMAGMin, zMAGMax, -30000, 30000))/30000.0;

 //normalize the magnetic vector
 float norm = sqrt( sq(xMAGMap) + sq(yMAGMap) + sq(zMAGMap));
 xMAGMap /=norm;
 yMAGMap /=norm;
 zMAGMap /=norm;

 float Pitch = (pitchAccel+pitchGyro)/2;
 float Roll = (rollAccel+rollGyro)/2;

 //compare "Applications of MAGic Sensors for Low Cost Compass Systems" by Michael J. Caruso
 //for the compensated Yaw equations...
 //http://www.ssec.honeywell.com/magnetic/datasheets/lowcost.pdf
 yawRaw=atan2( (-yMAGMap*cos(Roll) + zMAGMap*sin(Roll) ), (xMAGMap*cos(Pitch) + yMAGMap*sin(Pitch)*sin(Roll)+ zMAGMap*sin(Pitch)*cos(Roll)) ) *180/PI;
 YawU=atan2(-yMAGMap, xMAGMap) *180/PI;
}
http://dduino.blogspot.com all my Arduino/electronic projects!!!

{NEW} Getting Started, Learning, Reference + FAQ PDF!!:
[url]http://ar

Go Up