From 756938b190597e56a523cd1d707f7e793a0c03ec Mon Sep 17 00:00:00 2001
From: William Clark <wrvc96@gmail.com>
Date: Sat, 13 Jan 2024 19:51:57 +0000
Subject: [PATCH] gyro drift calibration/offset

---
 main.c    | 11 +++++++++-
 mpu6050.c | 62 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 mpu6050.h | 14 +++++++++++++
 3 files changed, 86 insertions(+), 1 deletion(-)

diff --git a/main.c b/main.c
index 0a1f1f8..4ed1187 100644
--- a/main.c
+++ b/main.c
@@ -16,11 +16,20 @@ int main() {
     mpu6050.dev.read = i2c_read;
     mpu6050.dev.write = i2c_write;
     mpu6050.dev.sleep = usleep;
-    
+
     if (mpu6050_init(&mpu6050)) {
         exit(1);
     }
 
+    if (mpu6050_calibrate_gyro(&mpu6050)) {
+        exit(1);
+    }
+
+    printf("[CALIB] G_x=%4.1f, G_y=%4.1f, G_z=%4.1f\n",
+        (float)mpu6050.offset.gyro_x / 10.f,
+        (float)mpu6050.offset.gyro_y / 10.f,
+        (float)mpu6050.offset.gyro_z / 10.f);
+
     mpu6050.cfg.gyro = MPU6050_GYRO_FS_250;
     mpu6050.cfg.acc = MPU6050_ACC_FS_2G;
     mpu6050.cfg.dlpl = 6;
diff --git a/mpu6050.c b/mpu6050.c
index efa8ca3..bce40b0 100644
--- a/mpu6050.c
+++ b/mpu6050.c
@@ -26,6 +26,7 @@ int mpu6050_init(mpu6050_t *mpu6050) {
     
     memset(&mpu6050->cfg, 0, sizeof mpu6050->cfg);
     memset(&mpu6050->data, 0, sizeof mpu6050->data);
+    memset(&mpu6050->offset, 0, sizeof mpu6050->offset);
 
     return err;
 }
@@ -90,6 +91,11 @@ int mpu6050_read_gyro(mpu6050_t *mpu6050) {
     mpu6050->data.gyro.y = (int16_t)(data[2] << 8 | data[3]) >> shift;
     mpu6050->data.gyro.z = (int16_t)(data[4] << 8 | data[5]) >> shift;
 
+    /* Apply offsets from calibration */
+    mpu6050->data.gyro.x += mpu6050->offset.gyro_x;
+    mpu6050->data.gyro.y += mpu6050->offset.gyro_y;
+    mpu6050->data.gyro.z += mpu6050->offset.gyro_z;
+
     return err;
 }
 
@@ -137,6 +143,11 @@ int mpu6050_read(mpu6050_t *mpu6050) {
     mpu6050->data.gyro.y = (int16_t)(data[10] << 8 | data[11]) >> shift_gyro;
     mpu6050->data.gyro.z = (int16_t)(data[12] << 8 | data[13]) >> shift_gyro;
 
+    /* Apply offsets from calibration */
+    mpu6050->data.gyro.x += mpu6050->offset.gyro_x;
+    mpu6050->data.gyro.y += mpu6050->offset.gyro_y;
+    mpu6050->data.gyro.z += mpu6050->offset.gyro_z;
+
     return err;
 }
 
@@ -185,6 +196,57 @@ int mpu6050_configure(mpu6050_t *mpu6050) {
     return err;
 }
 
+/* configures the device in the recommended mode for determining calibration */
+/* read gyro N times to determine a constant offset */
+/* this offset is stored in the mpu6050_t struct, */
+/* and is corrected for in all subsequent read operations */
+/* assumes stationary device */
+/* values tend not to change much over time, so can be re-used, */
+/* without calling this function again. */
+/* simply write the known offsets to the `mpu6050.offset' struct */
+int mpu6050_calibrate_gyro(mpu6050_t *mpu6050) {
+    uint8_t data[6];
+    int err = 0;
+    int i;
+    int16_t x, y, z;
+    uint8_t shift;
+    x = y = z = 0;
+
+    assert(mpu6050);
+
+    /* Set clock divider to 0 (1) */
+    err |= mpu6050->dev.write(REG_SMPLRT_DIV, 0);
+    /* Reset gyro */
+    err |= mpu6050->dev.write(REG_SIGNAL_PATH_RESET, 1 << 2);
+    /* Set digital low-pass filter level to 7 */
+    err |= mpu6050->dev.write(REG_CONFIG, 0);
+    /* Set 250 deg mode */
+    err |= mpu6050->dev.write(REG_GYRO_CONFIG, MPU6050_GYRO_FS_250 << 3);
+    /* No sleep */
+    err |= mpu6050->dev.write(REG_PWR_MGMT1, 0);
+
+    mpu6050->dev.sleep(250000); /* 250 ms */
+
+    shift = gyro_i16_shift(MPU6050_GYRO_FS_250);
+
+    for (i=0; i<MPU6050_CALIBRATION_SAMPLES; i++) {
+
+        err |= mpu6050->dev.read(REG_GYRO_XOUT_H, data, 6);
+        if (err) break;
+
+        x = x/2 + (((int16_t)(data[0] << 8 | data[1]) >> shift) / 2);
+        y = y/2 + (((int16_t)(data[2] << 8 | data[3]) >> shift) / 2);
+        z = z/2 + (((int16_t)(data[4] << 8 | data[5]) >> shift) / 2);
+
+        mpu6050->dev.sleep(1000); /* 1 ms */
+    }
+
+    mpu6050->offset.gyro_x = -x;
+    mpu6050->offset.gyro_y = -y;
+    mpu6050->offset.gyro_z = -z;
+
+    return err;
+}
 
 /* How much to shift down an i16 accel sample depending on full-scale mode set */
 static uint8_t acc_i16_shift(uint8_t fs) {
diff --git a/mpu6050.h b/mpu6050.h
index bc128c1..821a1b6 100644
--- a/mpu6050.h
+++ b/mpu6050.h
@@ -15,6 +15,8 @@
 #define MPU6050_ACC_FS_8G    0x02 /* ± 8g */
 #define MPU6050_ACC_FS_16G   0x03 /* ± 16g */
 
+#define MPU6050_CALIBRATION_SAMPLES 1000
+
 struct mpu6050_dev {
     int (*init)(void);
     int (*write)(uint8_t reg, uint8_t value);
@@ -61,10 +63,21 @@ struct mpu6050_data {
     int16_t temp;
 };
 
+/* calculated offsets to prevent drift etc */
+struct mpu6050_offset {
+    int16_t gyro_x;
+    int16_t gyro_y;
+    int16_t gyro_z;
+    int16_t acc_x;
+    int16_t acc_y;
+    int16_t acc_z;
+};
+
 struct mpu6050 {
     struct mpu6050_dev dev;
     struct mpu6050_config cfg;
     struct mpu6050_data data;
+    struct mpu6050_offset offset;
 };
 
 typedef struct mpu6050 mpu6050_t;
@@ -76,5 +89,6 @@ int mpu6050_read_gyro(mpu6050_t *mpu6050);
 int mpu6050_read_temp(mpu6050_t *mpu6050);
 int mpu6050_read(mpu6050_t *mpu6050);
 int mpu6050_configure(mpu6050_t *mpu6050);
+int mpu6050_calibrate_gyro(mpu6050_t *mpu6050);
 
 #endif
\ No newline at end of file