Source

ArduinoProjects / Sensors / Gyro / Gyro.ino

// Code by: Jody McAdams
// Twitter: @MegaJiXiang
// Website: http://www.jodymcadams.com
// Original code/instructions from: http://bildr.org/2011/06/l3g4200d-arduino/
// Kalman filter code from: http://interactive-matter.eu/blog/2009/12/18/filtering-sensor-data-with-a-kalman-filter/
// License: http://www.opensource.org/licenses/mit-license.php (Go crazy)
// L3G4200D data sheet: http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/DATASHEET/CD00265057.pdf

#include <Wire.h>

const int sensorRotationScaleDPS = 500; //(can be 250, 500, or 2000)

//Allow the gyroscope values to be filtered to reduce noise
#define ENABLE_FILTERING true

//Used for low pass filtering
const float lowPassFilteringFactor = 0.1f;

//If ENABLE_FILTERING is on, use Kalman filtering instead of low pass filtering
#define USE_KALMAN_FILTER true

//For Kalman filtering
const float Kalman_SensorNoise = 8.0f;  //Adjust this based on the noisiness sensor

typedef struct
{
  float q; //process noise covariance
  float x; //value
  float p; //estimation error covariance
  float k; //kalman gain
} kalman_state;

void kalman_init(kalman_state* pOut_state, float q, float p, float initialValue);
void kalman_update(kalman_state* state, float measurement);

#if USE_KALMAN_FILTER
kalman_state kalmanState_X;
kalman_state kalmanState_Y;
kalman_state kalmanState_Z;
#endif

#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24

//Sensitivity from data sheet in mdps/digit (millidegrees per second)
const float SoDR_250 = 8.75f;
const float SoDR_500 = 17.50f;
const float SoDR_2000 = 70.0f;

//Digital zero-rate level from data sheet in dps (degrees per second)
const float DVoff_250 = 10.0f;
const float DVoff_500 = 15.0f;
const float DVoff_2000 = 75.0f;

//Final scale factors based on data sheet numbers
const float scale_250 = SoDR_250/1000.0f;
const float scale_500 = SoDR_500/1000.0f;
const float scale_2000 = SoDR_2000/1000.0f;

//Save the scale value that will be used when the program starts
float scaleFactor = 0.0f;

int L3G4200D_Address = 105; //I2C address of the L3G4200D

float x = 0.0f;
float y = 0.0f;
float z = 0.0f;

float lastTimeMS;

//Program init
void setup()
{
  Wire.begin();
  Serial.begin(9600);

  Serial.println("starting up L3G4200D");
  setupL3G4200D(sensorRotationScaleDPS); // Configure L3G4200  - 250, 500 or 2000 deg/sec

  delay(1500); //wait for the sensor to be ready
  
  //Init Kalman states if needed
  #if USE_KALMAN_FILTER
    const float initialQ = 0.0625f;
    kalman_init(&kalmanState_X,initialQ,0.0f,0.0f);
    kalman_init(&kalmanState_Y,initialQ,0.0f,0.0f);
    kalman_init(&kalmanState_Z,initialQ,0.0f,0.0f);
  #endif
  
  lastTimeMS = millis();
}

float spamTimer = -1.0f;
float spamInterval = 0.125f; //seconds

//Program loop
void loop()
{
  const float currTimeMS = millis();
  const float timeElapsed = (currTimeMS-lastTimeMS)/1000.0f;
  lastTimeMS = currTimeMS;
  
  getGyroValues();  // This will update x, y, and z with new values
  
  spamTimer -= timeElapsed;
  if(spamTimer < 0.0f)
  {
    spamTimer = spamInterval;
    
    Serial.print("X:");
    Serial.print(x);
    Serial.print(", ");
 
    Serial.print("Y:");
    Serial.print(y);
    Serial.print(", ");

    Serial.print("Z:");
    Serial.println(z);
  }
}

void getGyroValues(){

  byte xMSB = readRegister(L3G4200D_Address, 0x29);
  byte xLSB = readRegister(L3G4200D_Address, 0x28);
  float xVal = scaleFactor * ((xMSB << 8) | xLSB);

  byte yMSB = readRegister(L3G4200D_Address, 0x2B);
  byte yLSB = readRegister(L3G4200D_Address, 0x2A);
  float yVal = scaleFactor * ((yMSB << 8) | yLSB);

  byte zMSB = readRegister(L3G4200D_Address, 0x2D);
  byte zLSB = readRegister(L3G4200D_Address, 0x2C);
  float zVal = scaleFactor * ((zMSB << 8) | zLSB);
  
  #if ENABLE_FILTERING
    #if USE_KALMAN_FILTER
      kalman_update(&kalmanState_X,xVal);
      kalman_update(&kalmanState_Y,yVal);
      kalman_update(&kalmanState_Z,zVal);
      
      x = kalmanState_X.x;
      y = kalmanState_Y.x;
      z = kalmanState_Z.x;
    #else
      //Use low pass filter
      x = FilterInput(x,xVal);
      y = FilterInput(y,yVal);
      z = FilterInput(z,zVal);
    #endif
  #else
    x = xVal;
    y = yVal;
    z = zVal;
  #endif
}

int setupL3G4200D(int scale){
  //From  Jim Lindblom of Sparkfun's code

  // Enable x, y, z and turn off power down:
  writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);

  // If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
  writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);

  // Configure CTRL_REG3 to generate data ready interrupt on INT2
  // No interrupts used on INT1, if you'd like to configure INT1
  // or INT2 otherwise, consult the datasheet:
  writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);

  // CTRL_REG4 controls the full-scale range, among other things:

  switch(scale)
  {
    case 250:
    {
      writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
      scaleFactor = scale_250;
      
      break;
    }
    case 500:
    {
      writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
      scaleFactor = scale_500;
      
      break;
    }
    case 2000:
    {
      writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
      scaleFactor = scale_2000;
      
      break;
    }
  }  

  // CTRL_REG5 controls high-pass filtering of outputs, use it
  // if you'd like:
  writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
}

void writeRegister(int deviceAddress, byte address, byte val) {
    Wire.beginTransmission(deviceAddress); // start transmission to device 
    Wire.write(address);       // send register address
    Wire.write(val);         // send value to write
    Wire.endTransmission();     // end transmission
}

int readRegister(int deviceAddress, byte address){

    int v;
    Wire.beginTransmission(deviceAddress);
    Wire.write(address); // register to read
    Wire.endTransmission();

    Wire.requestFrom(deviceAddress, 1); // read a byte

    while(!Wire.available()) {
        // waiting
    }

    v = Wire.read();
    return v;
}

//Low pass filtering

float FilterInput(float currInput, float newInput)
{
  return (newInput * lowPassFilteringFactor) + (currInput * (1.0f - lowPassFilteringFactor));
}

//Kalman filtering

void kalman_init(kalman_state* pOut_state, float q, float p, float initialValue)
{
  pOut_state->q = q;
  pOut_state->p = p;
  pOut_state->x = initialValue;
}

void kalman_update(kalman_state* state, float measurement)
{
  //prediction update
  //omit x = x
  state->p = state->p + state->q;

  //measurement update
  state->k = state->p / (state->p + Kalman_SensorNoise);
  state->x = state->x + state->k * (measurement - state->x);
  state->p = (1.0f - state->k) * state->p;
}