Commits

Jody McAdams  committed f8e597a

cleaned up some calibration stuff

  • Participants
  • Parent commits ff674d2

Comments (0)

Files changed (1)

File Sensors/Gyro/Gyro.ino

 
 #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;
+float calibrationTimer = 2.0f; //seconds
 
+#if ENABLE_CALIBRATION
+
+float calibrationScale = 32.0f;  
 enum CalibrationState
 {
   CalibrationState_CalculateInitialValues,
-  CalibrationState_MinimizeDrift,
   CalibrationState_Done,
 };
 
 CalibrationState calibrationState = CalibrationState_CalculateInitialValues;
+#endif
 
 const int sensorRotationScaleDPS = 500; //(can be 250, 500, or 2000)
 
 float spamInterval = 0.125f; //seconds
 
 #if ENABLE_CALIBRATION
-float calibrationTimer = calibrationTime;
-float minimizationTimer = minimizationTime;
 float driftX = 0.0f;
 float driftY = 0.0f;
 float driftZ = 0.0f;
   lastTimeMS = currTimeMS;
   
   getGyroValues();  // This will update x, y, and z with new values
-   
-  #if ENABLE_CALIBRATION
-  
+
+#if ENABLE_CALIBRATION
   switch(calibrationState)
   {
     case CalibrationState_CalculateInitialValues:
         driftX = driftAverageX;
         driftY = driftAverageY;
         driftZ = driftAverageZ;
-        
-        lastX = diffX;
-        lastY = diffY;
-        lastZ = diffZ;
 
-        calibrationState = CalibrationState_MinimizeDrift;
+        calibrationState = CalibrationState_Done;
       }
     
       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
+  }  
+#endif
   
   angleX += x*timeElapsed;
   angleY += y*timeElapsed;
   zVal = scaleFactor * ((zMSB << 8) | zLSB);
   
   #if ENABLE_CALIBRATION
-   xVal -= driftX*calibrationScaleX;
-   yVal -= driftY*calibrationScaleY;
-   zVal -= driftZ*calibrationScaleZ;
+   xVal -= driftX*calibrationScale;
+   yVal -= driftY*calibrationScale;
+   zVal -= driftZ*calibrationScale;
   #endif
   
   #if ENABLE_FILTERING