Merge branch 'devel'

config.cpp

@@ -18,6 +18,8 @@ uint8_t checksum(Config* config)
 void print_config()
 {
     Serial.printf("imu_enabled: %s\r\n", conf.imu_enabled?"true":"false");
+    Serial.printf("gyro_bias: %f %f %f\r\n", 
+        conf.gyro_bias[0], conf.gyro_bias[1], conf.gyro_bias[2]);
     Serial.printf("accel_bias: %f %f %f\r\n", 
         conf.accel_bias[0], conf.accel_bias[1], conf.accel_bias[2]);
     Serial.printf("mag_scale: %f %f %f\r\n", 

config.h

@@ -11,12 +11,14 @@ struct Config
     uint8_t checksum;
 
     bool imu_enabled;
+    float gyro_bias[3];
     float accel_bias[3];
     float mag_scale[3];
     float mag_bias[3];
 
     Config()
     : imu_enabled(false),
+      gyro_bias{0.0, 0.0, 0.0},
       accel_bias{0.0, 0.0, 0.0},
       mag_scale{1.0, 1.0, 1.0},
       mag_bias{0.0, 0.0, 0.0} {}

imu.cpp

@@ -34,15 +34,15 @@ void IMU::begin()
 	ax = ay = az = 0.0;
 	gx = gy = gz = 0.0;
 	qx = qy = qz = qw = 0.0;
-	abias_[0] = conf.accel_bias[0];
-	abias_[1] = conf.accel_bias[1];
-	abias_[2] = conf.accel_bias[2];
-	mscale_[0] = conf.mag_scale[0];
-	mscale_[1] = conf.mag_scale[1];
-	mscale_[2] = conf.mag_scale[2];
-	mbias_[0] = conf.mag_bias[0];
-	mbias_[1] = conf.mag_bias[1];
-	mbias_[2] = conf.mag_bias[2];
+
+	// Load config
+	for (int i = 0; i < 3; ++i)
+	{
+		abias_[i] = conf.accel_bias[i];
+		gbias_[i] = conf.gyro_bias[i];
+		mscale_[i] = conf.mag_scale[i];
+		mbias_[i] = conf.mag_bias[i];
+	}
 }
 
 void IMU::update()
@@ -64,9 +64,9 @@ void IMU::update()
 			az *= gravitationalAcceleration;
 
 			mpu_.readGyroData(gyro);
-			gx = (float)gyro[0] * gres_;
-			gy = (float)gyro[1] * gres_;
-			gz = (float)gyro[2] * gres_;
+			gx = (float)gyro[0] * gres_ - gbias_[0];
+			gy = (float)gyro[1] * gres_ - gbias_[1];
+			gz = (float)gyro[2] * gres_ - gbias_[2];
 			gx *= degreeToRadian;
 			gy *= degreeToRadian;
 			gz *= degreeToRadian;		
@@ -90,15 +90,18 @@ void IMU::calGyroAccel()
 
 	mpu_mutex_.lock();
 
-		mpu_.calibrateMPU9250(gyroBias, abias_);
+		mpu_.calibrateMPU9250(gbias_, abias_);
 		Serial.printf("[IMU] MPU Calibration:\r\n");
-		Serial.printf("Gyro bias: %f %f %f\r\n", gyroBias[0], gyroBias[1], gyroBias[2]);
+		Serial.printf("Gyro bias: %f %f %f\r\n", gbias_[0], gbias_[1], gbias_[2]);
 		Serial.printf("Accel bias: %f %f %f\r\n", abias_[0], abias_[1], abias_[2]);
 
 		mpu_.initMPU9250(AFS_4G, GFS_500DPS, 0x02);
 
 	mpu_mutex_.unlock();
 
+	conf.gyro_bias[0] = gbias_[0];
+	conf.gyro_bias[1] = gbias_[1];
+	conf.gyro_bias[2] = gbias_[2];
 	conf.accel_bias[0] = abias_[0];
 	conf.accel_bias[1] = abias_[1];
 	conf.accel_bias[2] = abias_[2];

imu.h

@@ -22,7 +22,7 @@ class IMU
 private:
     MPU9250 mpu_;
     float ares_, gres_, mres_;
-    float abias_[3], mscale_[3], mbias_[3];
+    float abias_[3], gbias_[3], mscale_[3], mbias_[3];
     float magCal_[3];
     hFramework::hMutex mpu_mutex_;
 };

imu/MPU9250.cpp

@@ -545,12 +545,12 @@ void MPU9250::calibrateMPU9250(float * dest1, float * dest2)
   data[5] = (-gyro_bias[2]/4)       & 0xFF;
 
 // Push gyro biases to hardware registers
-  writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
-  writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
-  writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
-  writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
-  writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
-  writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);
+  // writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
+  // writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
+  // writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
+  // writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
+  // writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
+  // writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);
 
 // Output scaled gyro biases for display in the main program
   dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity;