Commits

Jody McAdams committed ff674d2

Added a naive version of calibration that drastically reduces drift rate

  • Participants
  • Parent commits 62d87d8

Comments (0)

Files changed (1)

File Sensors/Gyro/Gyro.ino

 
 #include <Wire.h>
 
+#define ENABLE_CALIBRATION true
+
+const float calibrationTime = 2.0f; //seconds
+const float minimizationTime = 2.0f; //seconds
+const float initialCalibrationValue = 16.0f; //start at some decently large value
+float calibrationScaleX = initialCalibrationValue;  
+float calibrationScaleY = initialCalibrationValue;
+float calibrationScaleZ = initialCalibrationValue;
+const float calibrationRate = 1.25f;
+
+enum CalibrationState
+{
+  CalibrationState_CalculateInitialValues,
+  CalibrationState_MinimizeDrift,
+  CalibrationState_Done,
+};
+
+CalibrationState calibrationState = CalibrationState_CalculateInitialValues;
+
 const int sensorRotationScaleDPS = 500; //(can be 250, 500, or 2000)
 
 //Allow the gyroscope values to be filtered to reduce noise
 
 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
   lastTimeMS = millis();
 }
 
-float spamTimer = -1.0f;
-float spamInterval = 0.125f; //seconds
-
 float angleX = 0.0f;
 float angleY = 0.0f;
 float angleZ = 0.0f;
 
+float spamTimer = -1.0f;
+float spamInterval = 0.125f; //seconds
 
-//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;
-  }
-}
+#if ENABLE_CALIBRATION
+float calibrationTimer = calibrationTime;
+float minimizationTimer = minimizationTime;
+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();
-  const float timeElapsed = (currTimeMS-lastTimeMS)/1000.0f;
+  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;
+        
+        lastX = diffX;
+        lastY = diffY;
+        lastZ = diffZ;
+
+        calibrationState = CalibrationState_MinimizeDrift;
+      }
+    
+      return;
+    }
+    case CalibrationState_MinimizeDrift:
+    {
+      minimizationTimer -= timeElapsed;
+      
+      const float diffX = abs(xVal-lastX);
+      const float diffY = abs(yVal-lastY);
+      const float diffZ = abs(zVal-lastZ);
+      
+      if(diffX >= lastX)
+      {
+        calibrationScaleX *= calibrationRate;
+      }
+      
+      if(diffY >= lastY)
+      {
+        calibrationScaleY *= calibrationRate;
+      }
+      
+      if(diffZ >= lastZ)
+      {
+        calibrationScaleZ *= calibrationRate;
+      }
+      
+      lastX = diffX;
+      lastY = diffY;
+      lastZ = diffZ;
+      
+      if(minimizationTimer < 0.0f)
+      {
+        Serial.print("Calib. Scale X:");
+        Serial.print(calibrationScaleX,5);
+        Serial.print(", ");
+     
+        Serial.print("Calib. Scale Y:");
+        Serial.print(calibrationScaleY,5);
+        Serial.print(", ");
+    
+        Serial.print("Calib. Scale Z:");
+        Serial.println(calibrationScaleZ,5);
+        
+        calibrationState = CalibrationState_Done;
+      }
+      
+      return;
+    }
+  }
+  
+  #endif
   
   angleX += x*timeElapsed;
   angleY += y*timeElapsed;
 
   byte xMSB = readRegister(L3G4200D_Address, 0x29);
   byte xLSB = readRegister(L3G4200D_Address, 0x28);
-  float xVal = scaleFactor * ((xMSB << 8) | xLSB);
+  xVal = scaleFactor * ((xMSB << 8) | xLSB);
 
   byte yMSB = readRegister(L3G4200D_Address, 0x2B);
   byte yLSB = readRegister(L3G4200D_Address, 0x2A);
-  float yVal = scaleFactor * ((yMSB << 8) | yLSB);
+  yVal = scaleFactor * ((yMSB << 8) | yLSB);
 
   byte zMSB = readRegister(L3G4200D_Address, 0x2D);
   byte zLSB = readRegister(L3G4200D_Address, 0x2C);
-  float zVal = scaleFactor * ((zMSB << 8) | zLSB);
+  zVal = scaleFactor * ((zMSB << 8) | zLSB);
+  
+  #if ENABLE_CALIBRATION
+   xVal -= driftX*calibrationScaleX;
+   yVal -= driftY*calibrationScaleY;
+   zVal -= driftZ*calibrationScaleZ;
+  #endif
   
   #if ENABLE_FILTERING
     #if USE_KALMAN_FILTER
     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)
   pOut_state->x = initialValue;
 }
 
+
 void kalman_update(kalman_state* state, float measurement)
 {
   //prediction update