Jody McAdams avatar Jody McAdams committed c8c40f7

calibration is back! It calculates a the zero rates on the X, Y, and Z separately and uses them to filter out garbage. Works very well!

Comments (0)

Files changed (1)

Sensors/Gyro/Gyro.ino

 #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;
+//Sensitivity from data sheet in mdps/digit (millidegrees per second) with DPS conversion
+const float SoDR_250 = 8.75f/1000.0f;
+const float SoDR_500 = 17.50f/1000.0f;
+const float SoDR_2000 = 70.0f/1000.0f;
 
 //Digital zero-rate level from data sheet in dps (degrees per second)
-const float DVoff_250 = 10.0f;
+/*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;
+const float DVoff_2000 = 75.0f;*/
 
 //Save the scale value that will be used when the program starts
 float scaleFactor = 0.0f;
-float DVoff = 0.0f;
+
+//Digital zero-rate values (calculated from calibration instead of data sheet)
+float DVoff_X = 0.0f;
+float DVoff_Y = 0.0f;
+float DVoff_Z = 0.0f;
+
+const float DVOffScale = 1.2f;
 
 int L3G4200D_Address = 105; //I2C address of the L3G4200D
 
 float zVal = 0.0f;
 float tVal = 0.0f;
 
-//filtered sensor readings (if filtering is turned on)
-float x = 0.0f;
-float y = 0.0f;
-float z = 0.0f;
-
 float temperature = 0.0f;
 
 //Temporarily made up numbers
   Wire.begin();
   Serial.begin(9600);
 
-  Serial.println("starting up L3G4200D");
+  Serial.println("starting up L3G4200D...");
   setupL3G4200D(sensorRotationScaleDPS); // Configure L3G4200  - 250, 500 or 2000 deg/sec
 
   delay(1500); //wait for the sensor to be ready
+  Serial.println("L3G4200D started!");
+  
+  Serial.println("Calibrating...");
   
   lastTimeMS = millis();
 }
 
 float timeElapsed = 0.0f;
 
+bool isCalibrating = true;
+float calibrationTimer = 3.0f;
+
 //Program loop
 void loop()
 {
   
   getGyroValues();  // This will update x, y, and z with new values
   
-  angleX += x*timeElapsed;
-  angleY += y*timeElapsed;
-  angleZ += z*timeElapsed;
-  
-  angleX = WrapAngle(angleX);
-  angleY = WrapAngle(angleY);
-  angleZ = WrapAngle(angleZ);
+  if(isCalibrating == true)
+  {
+    const float absValX = abs(xVal);
+    const float absValY = abs(yVal);
+    const float absValZ = abs(zVal);
+    
+    if(absValX > DVoff_X)
+    {
+      DVoff_X = absValX;
+    }
+    
+    if(absValY > DVoff_Y)
+    {
+      DVoff_Y = absValY;
+    }
+    
+    if(absValZ > DVoff_Z)
+    {
+      DVoff_Z = absValZ;
+    }
+    
+    calibrationTimer -= timeElapsed;
+    
+    if(calibrationTimer < 0.0f)
+    {
+      Serial.println("Calibration complete!");
+      
+      isCalibrating = false;
+      
+      //Some fudging to account for the slight
+      //amount of movement that slips through
+      DVoff_X *= DVOffScale;
+      DVoff_Y *= DVOffScale;
+      DVoff_Z *= DVOffScale;
+    }
+    
+    return;
+  }
   
   spamTimer -= timeElapsed;
   if(spamTimer < 0.0f)
   }
 }
 
-void getGyroValues(){
+void getGyroValues()
+{
 
   tVal = readRegister(L3G4200D_Address, 0x26);
   
   //Interpolate from min to max temperature based on tempT
   temperature = Lerp(temperature_Min,temperature_Max,tempT);
   
+  //Get DPS values from registers
   byte xMSB = readRegister(L3G4200D_Address, 0x29);
   byte xLSB = readRegister(L3G4200D_Address, 0x28);
   xVal = scaleFactor * ((xMSB << 8) | xLSB);
   byte zLSB = readRegister(L3G4200D_Address, 0x2C);
   zVal = scaleFactor * ((zMSB << 8) | zLSB);
   
-  //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)
+  if(isCalibrating == false)
   {
-    xVal = 0.0f;
-  }
-  
-  if(abs(yVal) <= DVoff)
-  {
-    yVal = 0.0f;
-  }
-  
-  if(abs(zVal) <= DVoff)
-  {
-    zVal = 0.0f;
-  }
+      //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_X)
+      {
+        xVal = 0.0f;
+      }
+      
+      if(abs(yVal) <= DVoff_Y)
+      {
+        yVal = 0.0f;
+      }
+      
+      if(abs(zVal) <= DVoff_Z)
+      {
+        zVal = 0.0f;
+      }
+      
+      angleX += xVal*timeElapsed;
+      angleY += yVal*timeElapsed;
+      angleZ += zVal*timeElapsed;
+      
+      angleX = WrapAngle(angleX);
+      angleY = WrapAngle(angleY);
+      angleZ = WrapAngle(angleZ);
+  }  
 }
 
 int setupL3G4200D(int scale){
   // 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;
-      DVoff = DVoff_250;
+      scaleFactor = SoDR_250;
+      //DVoff = DVoff_250;
       
       break;
     }
     case 500:
     {
       writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
-      scaleFactor = scale_500;
-      DVoff = DVoff_500;
+      scaleFactor = SoDR_500;
+      //DVoff = DVoff_500;
       
       break;
     }
     case 2000:
     {
       writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
-      scaleFactor = scale_2000;
-      DVoff = DVoff_2000;
+      scaleFactor = SoDR_2000;
+      //DVoff = DVoff_2000;
       
       break;
     }
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.