ArduinoProjects / Sensors / Gyro / Gyro.ino

Full commit
Jody McAdams afbdcac 
Jody McAdams e31add5 

Jody McAdams dc0b235 
Jody McAdams 4c861df 
Jody McAdams afbdcac 
Jody McAdams e31add5 
Jody McAdams afbdcac 

Jody McAdams 4c861df 

Jody McAdams afbdcac 

Jody McAdams 4c861df 

Jody McAdams afbdcac 

Jody McAdams 4c861df 
Jody McAdams afbdcac 
Jody McAdams 4c861df 

Jody McAdams afbdcac 

Jody McAdams 4c861df 

Jody McAdams afbdcac 

Jody McAdams 4c861df 

Jody McAdams afbdcac 
Jody McAdams 4c861df 

Jody McAdams afbdcac 
Jody McAdams 4c861df 

Jody McAdams afbdcac 

Jody McAdams 4c861df 

Jody McAdams afbdcac 

Jody McAdams 4c861df 

Jody McAdams afbdcac 

Jody McAdams 4c861df 
Jody McAdams afbdcac 
Jody McAdams 4c861df 

// Code by: Jody McAdams
// Twitter: @MegaJiXiang
// Website:
// Original code/instructions from:
// Kalman filter code from:
// License: (Go crazy)
// L3G4200D data sheet:

#include <Wire.h>

#define lowPassFilteringFactor 0.1f

//Allow the gyroscope values to be filtered to reduce noise

//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 your 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);

kalman_state kalmanState_X;
kalman_state kalmanState_Y;
kalman_state kalmanState_Z;

#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()

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

  delay(1500); //wait for the sensor to be ready
  //Init Kalman states if needed
    const float initialQ = 0.0625f;
  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(", ");
    Serial.print(", ");


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);
      x = kalmanState_X.x;
      y = kalmanState_Y.x;
      z = kalmanState_Z.x;
      //Use low pass filter
      x = FilterInput(x,xVal);
      y = FilterInput(y,yVal);
      z = FilterInput(z,zVal);
    x = xVal;
    y = yVal;
    z = zVal;

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:

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

  // 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.write(address); // register to read

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

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

    v =;
    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;