Source

ArduinoProjects / Sensors / Gyro / Gyro.ino

Jody McAdams afbdcac 
Jody McAdams e31add5 

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


Jody McAdams ff674d2 

Jody McAdams f8e597a 
Jody McAdams ff674d2 
Jody McAdams f8e597a 


Jody McAdams ff674d2 






Jody McAdams f8e597a 
Jody McAdams ff674d2 
Jody McAdams 2a5285b 
Jody McAdams 4c861df 

Jody McAdams afbdcac 

Jody McAdams 2a5285b 


Jody McAdams 4c861df 



Jody McAdams 2a5285b 
Jody McAdams 4c861df 

















Jody McAdams afbdcac 

























Jody McAdams ff674d2 





Jody McAdams afbdcac 



Jody McAdams ff674d2 
Jody McAdams 4c861df 
Jody McAdams afbdcac 
Jody McAdams 4c861df 


Jody McAdams afbdcac 



Jody McAdams 2a5285b 
Jody McAdams afbdcac 
Jody McAdams 4c861df 










Jody McAdams afbdcac 

Jody McAdams 62d87d8 



Jody McAdams ff674d2 

Jody McAdams 62d87d8 
Jody McAdams ff674d2 













Jody McAdams 62d87d8 
Jody McAdams 4c861df 



Jody McAdams ff674d2 
Jody McAdams 4c861df 


Jody McAdams f8e597a 

Jody McAdams ff674d2 































































Jody McAdams f8e597a 
Jody McAdams ff674d2 



Jody McAdams f8e597a 

Jody McAdams 4c861df 
Jody McAdams 62d87d8 







Jody McAdams 4c861df 





Jody McAdams 62d87d8 
Jody McAdams 4c861df 
Jody McAdams afbdcac 
Jody McAdams 4c861df 
Jody McAdams 62d87d8 
Jody McAdams 4c861df 
Jody McAdams afbdcac 
Jody McAdams 4c861df 
Jody McAdams 62d87d8 
Jody McAdams 4c861df 
Jody McAdams afbdcac 





Jody McAdams ff674d2 
Jody McAdams afbdcac 


Jody McAdams ff674d2 
Jody McAdams afbdcac 


Jody McAdams ff674d2 


Jody McAdams f8e597a 


Jody McAdams ff674d2 
Jody McAdams afbdcac 

Jody McAdams 4c861df 













Jody McAdams afbdcac 












































































Jody McAdams ff674d2 





















Jody McAdams 4c861df 

Jody McAdams afbdcac 

Jody McAdams 4c861df 
Jody McAdams afbdcac 
Jody McAdams 4c861df 









Jody McAdams ff674d2 
Jody McAdams 4c861df 










  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
// 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>

#define ENABLE_CALIBRATION true

float calibrationTimer = 2.0f; //seconds

#if ENABLE_CALIBRATION

float calibrationScale = 32.0f;  
enum CalibrationState
{
  CalibrationState_CalculateInitialValues,
  CalibrationState_Done,
};

CalibrationState calibrationState = CalibrationState_CalculateInitialValues;
#endif

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

//actual sensor readings
float xVal = 0.0f;
float yVal = 0.0f;
float zVal = 0.0f;

//filtered sensor readings (if filtering is turned on)
float x = 0.0f;
float y = 0.0f;
float z = 0.0f;

//last time in ms
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 angleX = 0.0f;
float angleY = 0.0f;
float angleZ = 0.0f;

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

#if ENABLE_CALIBRATION
float driftX = 0.0f;
float driftY = 0.0f;
float driftZ = 0.0f;
float driftAverageX = 0.0f;
float driftAverageY = 0.0f;
float driftAverageZ = 0.0f;
float lastX = 0.0f;
float lastY = 0.0f;
float lastZ = 0.0f;
float numCalibrationReadings = 0.0f;
#endif

float timeElapsed = 0.0f;

//Program loop
void loop()
{
  const float currTimeMS = millis();
  timeElapsed = (currTimeMS-lastTimeMS)/1000.0f;
  lastTimeMS = currTimeMS;
  
  getGyroValues();  // This will update x, y, and z with new values

#if ENABLE_CALIBRATION
  switch(calibrationState)
  {
    case CalibrationState_CalculateInitialValues:
    {
      calibrationTimer -= timeElapsed;
    
      const float diffX = xVal-lastX;
      const float diffY = yVal-lastY;
      const float diffZ = zVal-lastZ;
      
      lastX = xVal;
      lastY = yVal;
      lastZ = zVal;
      
      Serial.print("Diff X:");
      Serial.print(diffX,5);
      Serial.print(", ");
   
      Serial.print("Diff Y:");
      Serial.print(diffY,5);
      Serial.print(", ");
  
      Serial.print("Diff Z:");
      Serial.println(diffZ,5);
      
      driftAverageX += diffX;
      driftAverageY += diffY;
      driftAverageZ += diffZ;
      
      ++numCalibrationReadings;
      
      //Does this when finished
      if(calibrationTimer < 0.0f)
      { 
        Serial.print("Total Drift X:");
        Serial.print(driftAverageX,5);
        Serial.print(", ");
     
        Serial.print("Total Drift Y:");
        Serial.print(driftAverageY,5);
        Serial.print(", ");
    
        Serial.print("Total Drift Z:");
        Serial.println(driftAverageZ,5);
        
        driftAverageX /= numCalibrationReadings;
        driftAverageY /= numCalibrationReadings;
        driftAverageZ /= numCalibrationReadings;
        
        Serial.print("Drift Avg X:");
        Serial.print(driftAverageX,5);
        Serial.print(", ");
     
        Serial.print("Drift Avg Y:");
        Serial.print(driftAverageY,5);
        Serial.print(", ");
    
        Serial.print("Drift Avg Z:");
        Serial.println(driftAverageZ,5);
        
        driftX = driftAverageX;
        driftY = driftAverageY;
        driftZ = driftAverageZ;

        calibrationState = CalibrationState_Done;
      }
    
      return;
    }
  }  
#endif
  
  angleX += x*timeElapsed;
  angleY += y*timeElapsed;
  angleZ += z*timeElapsed;
  
  angleX = WrapAngle(angleX);
  angleY = WrapAngle(angleY);
  angleZ = WrapAngle(angleZ);
  
  spamTimer -= timeElapsed;
  if(spamTimer < 0.0f)
  {
    spamTimer = spamInterval;
    
    Serial.print("X:");
    Serial.print(angleX);
    Serial.print(", ");
 
    Serial.print("Y:");
    Serial.print(angleY);
    Serial.print(", ");

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

void getGyroValues(){

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

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

  byte zMSB = readRegister(L3G4200D_Address, 0x2D);
  byte zLSB = readRegister(L3G4200D_Address, 0x2C);
  zVal = scaleFactor * ((zMSB << 8) | zLSB);
  
  #if ENABLE_CALIBRATION
   xVal -= driftX*calibrationScale;
   yVal -= driftY*calibrationScale;
   zVal -= driftZ*calibrationScale;
  #endif
  
  #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;
}


//If the angle goes beyond 360, wrap it around
float WrapAngle(float angle)
{
  static const float maxAngle = 360.0f;
  
  float finalAngle;
  if(angle > maxAngle)
  {
    return angle - maxAngle;
  }
  else if(angle < -maxAngle)
  {
    return angle + maxAngle;
  }
  else
  {
    return angle;
  }
}


//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;
}
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.