Check my pololu mini 9 IMU code

ive been trying create libraryless arduino program interface pololu miniimu9(http://www.pololu.com/catalog/product/1265) in attempt learn more spi/imus. has l3g4200d(gyro) datasheet , lsm303dlm(acc+mag). can on 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, 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 setup code here, 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 main code here, 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 msb of address accelerometer
 // 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 msb of address gyro
 // 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 stable values
 for(int = 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! (float)!
}


void calculations()
{
 //calculate pitch in degrees measured accelerometers. note 8g 11 bits gives 256 bits/g
 pitchaccel = atan2((accy - baccy) / 256, (accz - baccz) / 256) * 360.0 / (2*pi);
 //calculate pitch in degrees measured gyros, angle = angle + gyro reading * time step.
 pitchgyro = pitchgyro + ((gyrox - bgyrox) / 70) * 0.02; //0.02 = gyro angle/sec
 //ditto 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 required normalize magnetic vector
 //get min , max reading 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 incoming data -1 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 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 low cost compass systems" michael j. caruso
 //for 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;
}

Arduino Forum > Using Arduino > Programming Questions > Check my pololu mini 9 IMU code

arduino