Pololu MiniIMU-9 Code

I finally got around to finishing some code for my Arduino that interfaces with the Pololu MiniIMU9 to get gyro, accelerometer and magnetometer data. But, that would be boring alone and so i implemented pitch, roll and yaw(with mag), too. Also, I somehow managed to successfully implement Kalman filtering to combine gyro and acc data. But, i did have a few issues as i was writing this code with setting up the Wire interface and calculating pitch/roll/yaw from gyro, acc and mag data.

Hopefully, with this post i will explain how i did this so that others can apply it to other IMUs etc. First thing you need to do is enable the sensors through the registers. Most times this enable register (often the first register) is named similar to mine:  CTRL_REG1. Looking the datasheet for the sensor, you can find what value this register needs to be for your startup settings. This is how you send values to registers:

Wire.beginTransmission(address);
Wire.write(register);
Wire.write(value);
Wire.endTransmission();

In my code they were made into functions:

writeGyroReg(L3G4200D_CTRL_REG1, 0b10101111); //enable gyro, 0b10101111, 1010- 400hz ODR/50hz cutoff, 1111- default(enable all axis/normal mode)

writeAccReg(LSM303_CTRL_REG1_A, 0b00110111); //enable acc, 0b00110111, 001- normal mode, 10- 400hz ODR, 111- default(enable all axis)

writeMagReg(LSM303_MR_REG_M, 0x00); //enable mag

Next, you need to look for sensitivity; it will given for different modes (2000dps, 1000dps etc) for the gyro so you need to use the sensitivity factor that lines up with the dps. Same for AccGain, if you use 8 gauss mode, you need to use the sensitivity for that mode. This code is how i made sure my gyro/acc was on the right mode so that my Gyro/AccGain lined up in the calculation part of the code(explained later):

//gyro settings
  writeGyroReg(CTRL_REG4, 0b00110001); //0-continous update, 0- little endian, 11 2000dps, 0- blank,  00- self test disabled, 0 spi 4 wire(if used)

  //acc settings
  writeAccReg(CTRL_REG4_A, 0b00110000); //0- continuous update, 0- little endian, 11- 8g scale, 00 default, 0- blank, 0- self test disabled

So it seems that registers 1(enable, data rate, filters) and 4(update mode, sensitivity etc) are the most important. I still need to figure out how to choose the best  register settings for ODR and high and low pass filters.

The next thing you need to do is get some values from the sensors so that you can determine the level position values and use that as a bias throughout the code. Simply get xyz values from the Acc and Gyro a 100 times, or so, and then divide by 100 to get an average.

But, you need to read the values from the sensors first, and to do that something similar to enabling the sensors is used. It first begins the communication with the sensor, and then writes a value to the output register signalling that it wants data. Then it waits for the request data and then moves them to bytes. The bytes are then recombined into whole numbers into integers of the x/y/z axis.

Wire.beginTransmission(address);
  Wire.write(LSM303_OUT_X_L_A | (1 << 7));
  Wire.endTransmission();
  Wire.requestFrom(address, 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();

  ACCy = -((xha << 8 | xla) >> 4); //reversed y axis
  ACCx = -((yha << 8 | yla) >> 4); //reversed x
  ACCz = (zha << 8 | zla) >> 4;

Now that you have the data into easy to work with integers, you can calculate the pitch, roll, and yaw etc. That in its self is easy; all you have to do is: pitchGyro = (GYROx - bGYROx) / GyroGain; or rollGyro = (GYROy - bGYROy) / GyroGain; but that wont provide very accurate results. GYROx is the value received from the gyro, bGYROx is the bias for the gyro calculated when the IMU was first turned on, and GyroGain is the factor that gives the value in DEG/s from the raw sensor value. This can be found in the data sheet as i mentioned before.

To provide accurate results, you need to combine acc and gyro values with a kalman filter. The Kalman filter has two main parts. One calculates pitch and roll individually based on acc and gyro:

Gyro pitch = (Gyro pitch  + ((GyroXvalue -  GyroXbias) / GyroGain)) * timeStep;

Accel pitch = (atan2((AccYvalue -  AccYbias) / AccGain, (AccZvalue  -  AccZbias) / AccGain) * 180.0) / PI;
Pitch prediction = Pitch prediction + ((GyroXvalue -  GyroXbias) / GyroGain) * timeStep;

Gyro roll = (rollGyro + ((GyroYvalue -  GyroYbias) / GyroGain)) * timeStep; //gyro roll in deg
Acc roll = (atan2(( AccXvalue -  AccXbias ) / AccGain, (AccZvalue -  AccZbias) / AccGain) * 180.0) / PI;
Roll prediction = rollPrediction - ((GyroYvalue -  GyroYbias) / GyroGain) * timeStep;

timestep is  how long one loop of the program is, and (*180.0) / PI is converting radians into degrees.

and the other, which is the actual filter which combines the gyro and acc data to get pitch/roll. i dont really know much about this, so i cant really explain.

void Kalman() //kalman filter for pitch / roll
{
  Pxx += timeStep * (2 * Pxv + timeStep * Pvv);
  Pxv += timeStep * Pvv;
  Pxx += timeStep * giroVar;
  Pvv += timeStep * deltaGiroVar;
  kx = Pxx * (1 / (Pxx + accelVar));
  kv = Pxv * (1 / (Pxx + accelVar));

  pitchPrediction += (pitchAccel - pitchPrediction) * kx;
  rollPrediction += (rollAccel - rollPrediction) * kx;

  Pxx *= (1 - kx);
  Pxv *= (1 - kx);
  Pvv -= kv * Pxv;
}

Next up is to calculate yaw accurately with the help of the mag. MAGx/y/z are the raw values from the sensor.

void YawCalc() // calculate yaw from mag
{
  //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;

  //new calcs:
  float xh = xMAGMap * cos(pitchPrediction) + zMAGMap * sin(pitchPrediction);
  float yh = xMAGMap * sin(rollPrediction) * sin(pitchPrediction) + yMAGMap * cos(rollPrediction) - zMAGMap * sin(rollPrediction) * cos(pitchPrediction);
  float zh = -xMAGMap * cos(rollPrediction) * sin(pitchPrediction) + yMAGMap * sin(rollPrediction) + zMAGMap * cos(rollPrediction) * cos(pitchPrediction);

  yawRaw = 180 * atan2(yh, xh)/PI;
  if (yh >= 0)
  {
    //do nothing, yaw value is ok
  }
  else
  {
    yawRaw += 360;
  }
}

Code:

#include <Wire.h>

//I2C addressses for IMU
#define GYR_ADDRESS (0xD2 >> 1)
#define MAG_ADDRESS (0x3C >> 1)
#define ACC_ADDRESS (0x30 >> 1)

//Gyro, ACC, Mag enable registers
#define L3G4200D_CTRL_REG1 0x20
#define LSM303_CTRL_REG1_A 0x20
#define LSM303_MR_REG_M 0x02

//acc settings
#define CTRL_REG4_A 0x23

//gyrto settings
#define CTRL_REG4 0x23

//Gyro, Acc, Mag output registers
#define L3G4200D_OUT_X_L 0x28
#define LSM303_OUT_X_L_A 0x28
#define LSM303_OUT_X_H_M 0x03

//ACC Reg data
int ACCx;
int ACCy;
int ACCz;
//Bias
int bACCx;
int bACCy;
int bACCz;

//Gyro Reg data
int GYROx;
int GYROy;
int GYROz;
//Bias
int bGYROx;
int bGYROy;
int bGYROz;

//MAG REG data
int MAGx;
int MAGy;
int MAGz;

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


//calculations
int pitchAccel;
int pitchGyro;
int rollAccel;
int rollGyro;
//float YawU;

//gyro/acc gain-converts raw values, gyro deg/s, acc to Gs
#define AccGain 3.9      //8g
#define GyroGain 70     //2000dps

//kalman,
float giroVar = 0.1;
float deltaGiroVar = 0.1;
float accelVar = 5;
float Pxx = 0.1; // angle variance
float Pvv = 0.1; // angle change rate variance
float Pxv = 0.1; // angle and angle change rate covariance
float kx, kv;

//final values
float pitchPrediction = 0; //Output of Kalman filter, final pitch value
float rollPrediction = 0;  //Output of Kalman filter, final roll value
float yawRaw=0; //final yaw value. yaw doesnt go through kalman! has its own calc.

//time, used in kalman filtering to keep a constant loop time
unsigned long timer = 0;
unsigned long timer1 = 0;
float timeStep = 0.02;          //20ms. Need a time step value for integration of gyro angle from angle/sec

void setup()
{
  Serial.begin(115200); //begin serial comm.

  Wire.begin(); //start Wire for IMU

  writeGyroReg(L3G4200D_CTRL_REG1, 0b10101111); //enable gyro, 0b10101111, 1010- 400hz ODR/50hz cutoff, 1111- default(enable all axis/normal mode)
  writeAccReg(LSM303_CTRL_REG1_A, 0b00110111); //enable acc, 0b00110111, 001- normal mode, 10- 400hz ODR, 111- default(enable all axis)
  writeMagReg(LSM303_MR_REG_M, 0x00); //enable mag

  //gyro settings
  writeGyroReg(CTRL_REG4, 0b00110001); //0-continous update, 0- little endian, 11 2000dps, 0- blank,  00- self test disabled, 0 spi 4 wire(if used)

  //acc settings
  writeAccReg(CTRL_REG4_A, 0b00110000); //0- continuous update, 0- little endian, 11- 8g scale, 00 default, 0- blank, 0- self test disabled

  CalibrateIMU(); //calibrate the IMU for level starting pos.
}

void loop() {
  timer = millis(); //loop begin time

  //read sensors
  readGyro();
  readAcc();
  readMag();

  //calcualte pitch, roll, yaw, kalman etc
  Calculations();

  //print values
  PrintVals();

  timer1 = millis(); //loop end time
  delay(((timeStep * 1000)-(timer1-timer))); //delay so loop lasts 20msec, (timestep(.02) * 1000 = msec) - how long loop took
}

void readGyro() // get x, y, z values from gyro
{
  Wire.beginTransmission(GYR_ADDRESS);
  Wire.write(L3G4200D_OUT_X_L | (1 << 7));
  Wire.endTransmission();
  Wire.requestFrom(GYR_ADDRESS, 6);

  while (Wire.available() < 6);

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

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

void readAcc() // get x, y, z values from accelerometer
{
  Wire.beginTransmission(ACC_ADDRESS);
  Wire.write(LSM303_OUT_X_L_A | (1 << 7));
  Wire.endTransmission();
  Wire.requestFrom(ACC_ADDRESS, 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();

  ACCy = -((xha << 8 | xla) >> 4); //reversed y axis
  ACCx = -((yha << 8 | yla) >> 4); //reversed x
  ACCz = (zha << 8 | zla) >> 4;
}

void readMag() //get mag x, y, z values
{
  Wire.beginTransmission(MAG_ADDRESS);
  Wire.write(LSM303_OUT_X_H_M);
  Wire.endTransmission();
  Wire.requestFrom(MAG_ADDRESS, 6);

  while (Wire.available() < 6);

  byte xhm = Wire.read();
  byte xlm = Wire.read();

  byte yhm, ylm, zhm, zlm;

  zhm = Wire.read();
  zlm = Wire.read();
  yhm = Wire.read();
  ylm = Wire.read();

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

void CalibrateIMU() //get level value bias of IMU sensors
{
  //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 for acc + gyro
  {
    readGyro();
    readAcc();
    readMag();

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

    tACCx += ACCx;
    tACCy += ACCy;
    tACCz += ACCz;
    delay(8);
  }

  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 roll/pitch for acc/gyro, remove level bias. kalman filtering for pitch/roll, calc yaw
{
  /*
  Gyro in deg/s
   pitchGyro = (GYROx - bGYROx) / GyroGain;
   rollGyro = (GYROy - bGYROy) / GyroGain;
   */

  pitchGyro = (pitchGyro + ((GYROx - bGYROx) / GyroGain)) * timeStep; //gyro pitch in deg
  pitchAccel = (atan2((ACCy - bACCy) / AccGain, (ACCz - bACCz) / AccGain) * 180.0) / PI;
  pitchPrediction = pitchPrediction + ((GYROx - bGYROx) / GyroGain) * timeStep;

  rollGyro = (rollGyro + ((GYROy - bGYROy) / GyroGain)) * timeStep; //gyro roll in deg
  rollAccel = (atan2((ACCx - bACCx) / AccGain, (ACCz - bACCz) / AccGain) * 180.0) / PI;
  rollPrediction = rollPrediction - ((GYROy - bGYROy) / GyroGain) * timeStep;

  YawCalc();  //calc yaw with mag!

  Kalman(); //predict pitch, roll
}

void YawCalc() // calculate yaw from mag
{
  //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;

  //new calcs:
  float xh = xMAGMap * cos(pitchPrediction) + zMAGMap * sin(pitchPrediction);
  float yh = xMAGMap * sin(rollPrediction) * sin(pitchPrediction) + yMAGMap * cos(rollPrediction) - zMAGMap * sin(rollPrediction) * cos(pitchPrediction);
  float zh = -xMAGMap * cos(rollPrediction) * sin(pitchPrediction) + yMAGMap * sin(rollPrediction) + zMAGMap * cos(rollPrediction) * cos(pitchPrediction);

  yawRaw = 180 * atan2(yh, xh)/PI;
  if (yh >= 0)
  {
    //do nothing, yaw value is ok
  }
  else
  {
    yawRaw += 360;
  }
}

void Kalman() //kalman filter for pitch / roll
{
  Pxx += timeStep * (2 * Pxv + timeStep * Pvv);
  Pxv += timeStep * Pvv;
  Pxx += timeStep * giroVar;
  Pvv += timeStep * deltaGiroVar;
  kx = Pxx * (1 / (Pxx + accelVar));
  kv = Pxv * (1 / (Pxx + accelVar));

  pitchPrediction += (pitchAccel - pitchPrediction) * kx;
  rollPrediction += (rollAccel - rollPrediction) * kx;

  Pxx *= (1 - kx);
  Pxv *= (1 - kx);
  Pvv -= kv * Pxv;
}

//write stuff to the snsor registers
void writeGyroReg(byte reg, byte value)
{
  Wire.beginTransmission(GYR_ADDRESS);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

void writeAccReg(byte reg, byte value)
{
  Wire.beginTransmission(ACC_ADDRESS);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

void writeMagReg(byte reg, byte value)
{
  Wire.beginTransmission(MAG_ADDRESS);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

void PrintVals()
{
  /*
  Serial.print("RawPitchGyro:");
   Serial.print(pitchGyro);
   Serial.print(" RawPitchAccel:");
   Serial.print(pitchAccel);
   Serial.print(" RawRollGyro:");
   Serial.print(rollGyro);
   Serial.print(" RawRollAccel:");
   Serial.print(rollAccel);
   Serial.print(" PitchPredict:");
   Serial.print(pitchPrediction);
   Serial.print(" RollPredict:");
   Serial.print(rollPrediction);
   Serial.print(" Yaw:");
   Serial.print(yawRaw);
   Serial.println();
   */

  //IMU grapher processing
  Serial.print(map(pitchPrediction, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(rollPrediction, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(yawRaw, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(pitchGyro, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(pitchAccel, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(rollGyro, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(rollAccel, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(pitchPrediction, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(rollPrediction, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(yawRaw, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(rollGyro, -180,180,0,500));
  Serial.println();

}

Next is to implement some PID and make some balance bot magic!