From 4ce9fc9f9d7a37f911cfd367febb65859be90fe7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= <bsowa123@gmail.com>
Date: Mon, 21 Oct 2019 18:44:54 +0200
Subject: [PATCH] add gyro bias to persistent config

---
 config.cpp      |  2 ++
 config.h        |  2 ++
 imu.cpp         | 31 +++++++++++++++++--------------
 imu.h           |  2 +-
 imu/MPU9250.cpp | 12 ++++++------
 5 files changed, 28 insertions(+), 21 deletions(-)

diff --git a/config.cpp b/config.cpp
index e9e9f9a..a4eca8e 100644
--- a/config.cpp
+++ b/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", 
diff --git a/config.h b/config.h
index e537145..d5ceff4 100644
--- a/config.h
+++ b/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} {}
diff --git a/imu.cpp b/imu.cpp
index cdb8c64..a12336b 100644
--- a/imu.cpp
+++ b/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];
diff --git a/imu.h b/imu.h
index 5393bba..7fc5e20 100644
--- a/imu.h
+++ b/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_;
 };
diff --git a/imu/MPU9250.cpp b/imu/MPU9250.cpp
index 127c9cf..a879aeb 100644
--- a/imu/MPU9250.cpp
+++ b/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;