Pages: [1]   Go Down
Author Topic: Check my pololu mini 9 IMU code  (Read 883 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Sr. Member
****
Karma: 1
Posts: 486
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
#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;
}
« Last Edit: April 17, 2012, 04:07:34 pm by sirbow2 » Logged

http://dduino.blogspot.com all my Arduino/electronic projects!!!

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

Pages: [1]   Go Up
Jump to: