Go Down

Topic: Check my pololu mini 9 IMU code (Read 990 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
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy