Implement IMU calibration

Author: Zhiquan Yeo

include/imu.h

@@ -14,6 +14,7 @@ void imuSetEnabled(bool enabled);
 bool imuIsEnabled();
 
 void imuInit(uint8_t addr, TwoWire *theWire);
+void imuCalibrate(unsigned long calibrationTime);
 
 bool imuPeriodic();
 

src/imu.cpp

@@ -1,5 +1,7 @@
 #include "imu.h"
 
+#define IMU_DEFAULT_CALIBRATION_TIME_MS 3000
+
 namespace xrp {
 
 unsigned long _imuUpdatePeriod = 1000 / IMU_UPDATE_RATE_HZ;
@@ -40,6 +42,11 @@ void imuInit(uint8_t addr, TwoWire *theWire) {
     _imuReady = true;
     Serial.println("--- IMU ---");
     Serial.println("LSM6DSOX detected");
+    
+    Serial.println("Setting update rate to 208Hz");
+    _lsm6.setGyroDataRate(LSM6DS_RATE_208_HZ);
+    _lsm6.setAccelDataRate(LSM6DS_RATE_208_HZ);
+
     Serial.print("Accel Range: ");
     switch (_lsm6.getAccelRange()) {
       case LSM6DS_ACCEL_RANGE_2_G:
@@ -79,6 +86,46 @@ void imuInit(uint8_t addr, TwoWire *theWire) {
   }
 }
 
+void imuCalibrate(unsigned long calibrationTime) {
+  unsigned long loopDelayTime = 1000 / 208;
+
+  if (calibrationTime == 0) {
+    calibrationTime = IMU_DEFAULT_CALIBRATION_TIME_MS;
+  }
+
+  Serial.printf("[IMU] Beginning calibration. Running for %u ms\n", calibrationTime);
+  
+  float gyroAvgValues[3] = {0, 0, 0};
+  int numVals = 0;
+
+  unsigned long startTime = millis();
+  while (millis() < startTime + calibrationTime) {
+    // Get IMU data
+    sensors_event_t accel;
+    sensors_event_t gyro;
+    sensors_event_t temp;
+
+    _lsm6.getEvent(&accel, &gyro, &temp);
+
+    gyroAvgValues[0] += _radToDeg(gyro.gyro.x);
+    gyroAvgValues[1] += _radToDeg(gyro.gyro.y);
+    gyroAvgValues[2] += _radToDeg(gyro.gyro.z);
+
+    numVals++;
+    delay(loopDelayTime);
+  }
+
+  _gyroOffsetsDegPerSec[0] = gyroAvgValues[0] / numVals;
+  _gyroOffsetsDegPerSec[1] = gyroAvgValues[1] / numVals;
+  _gyroOffsetsDegPerSec[2] = gyroAvgValues[2] / numVals;
+
+  Serial.printf("[IMU] Offsets: X(%f) Y(%f) Z(%f)\n",
+      _gyroOffsetsDegPerSec[0],
+      _gyroOffsetsDegPerSec[1],
+      _gyroOffsetsDegPerSec[2]);
+  Serial.println("[IMU] Calibration Complete");
+}
+
 bool imuPeriodic() {
   if (!_imuReady) return false;
   if (!_imuEnabled) return false;

src/main.cpp

@@ -108,7 +108,8 @@ void setup() {
   Serial.println("[IMU] Initializing IMU");
   xrp::imuInit(IMU_I2C_ADDR, &Wire1);
 
-  // TODO Calibrate IMU
+  Serial.println("[IMU] Beginning IMU calibration");
+  xrp::imuCalibrate(0); // Use the default calibration time
 
   // Busy-loop if there's no WiFi hardware
   if (WiFi.status() == WL_NO_MODULE) {