1. Jody McAdams
  2. ArduinoProjects

Commits

Jody McAdams  committed 9b4699e

new filterless, calibration less code that uses the magical DVOff values from the data sheet

  • Participants
  • Parent commits 28170e7
  • Branches default

Comments (0)

Files changed (1)

File Sensors/Gyro/Gyro.ino

View file
 // 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>
 #include <MathUtil.h>
 
-//Allow the gyroscope values to be filtered to reduce noise
-#define ENABLE_FILTERING false
-
-//Enable auto-calibration of gyroscope
-#define ENABLE_CALIBRATION true
-
-float calibrationTimer = 2.0f; //seconds
-
-#if ENABLE_CALIBRATION
-
-const float calibrationScale = 32.0f; //hacky way to adjust the calibration effect (32 works well for me)
-
-enum CalibrationState
-{
-  CalibrationState_CalculateInitialValues,
-  CalibrationState_Done,
-};
-
-CalibrationState calibrationState = CalibrationState_CalculateInitialValues;
-#endif
-
 const int sensorRotationScaleDPS = 250; //(can be 250, 500, or 2000)
 
-//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 false
-
-//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
 
 //Save the scale value that will be used when the program starts
 float scaleFactor = 0.0f;
+float DVoff = 0.0f;
 
 int L3G4200D_Address = 105; //I2C address of the L3G4200D
 
 
   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
 
-#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
   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;
   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 the DPS values are less than or equal to the Digital zero-rate levels from
+  //the datasheet, set the values to 0
+  if(abs(xVal) <= DVoff)
+  {
+    xVal = 0.0f;
+  }
   
-  #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
+  if(abs(yVal) <= DVoff)
+  {
+    yVal = 0.0f;
+  }
+  
+  if(abs(zVal) <= DVoff)
+  {
+    zVal = 0.0f;
+  }
 }
 
 int setupL3G4200D(int scale){
     {
       writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
       scaleFactor = scale_250;
+      DVoff = DVoff_250;
       
       break;
     }
     {
       writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
       scaleFactor = scale_500;
+      DVoff = DVoff_500;
       
       break;
     }
     {
       writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
       scaleFactor = scale_2000;
+      DVoff = DVoff_2000;
       
       break;
     }
 }
 
 
-//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;
-}
-