From abaae7e5fa2615c7ca2b71d5eaadc8647a5bca52 Mon Sep 17 00:00:00 2001
From: William Clark <wrvc96@gmail.com>
Date: Sun, 14 Jan 2024 19:02:42 +0000
Subject: [PATCH] compensate constant gyro drift with on-device registers
---
main.c | 9 +++---
mpu6050.c | 82 ++++++++++++++++++++++++++++++++++++++---------------
mpu6050.h | 3 +-
registers.h | 12 ++++++++
4 files changed, 77 insertions(+), 29 deletions(-)
diff --git a/main.c b/main.c
index 4ed1187..9e7a831 100644
--- a/main.c
+++ b/main.c
@@ -21,14 +21,13 @@ int main() {
exit(1);
}
- if (mpu6050_calibrate_gyro(&mpu6050)) {
+ if (mpu6050_reset(&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);
+ if (mpu6050_calibrate_gyro(&mpu6050)) {
+ exit(1);
+ }
mpu6050.cfg.gyro = MPU6050_GYRO_FS_250;
mpu6050.cfg.acc = MPU6050_ACC_FS_2G;
diff --git a/mpu6050.c b/mpu6050.c
index bce40b0..952b3e5 100644
--- a/mpu6050.c
+++ b/mpu6050.c
@@ -91,11 +91,6 @@ 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;
}
@@ -143,11 +138,6 @@ 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;
}
@@ -198,12 +188,8 @@ int mpu6050_configure(mpu6050_t *mpu6050) {
/* 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 */
+/* write the offsets to device, which will in turn consider them for all */
+/* subsequent gyro reading operations. Assumes device is stationary. */
int mpu6050_calibrate_gyro(mpu6050_t *mpu6050) {
uint8_t data[6];
int err = 0;
@@ -220,14 +206,14 @@ int mpu6050_calibrate_gyro(mpu6050_t *mpu6050) {
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);
+ /* Set 1000 deg mode */
+ err |= mpu6050->dev.write(REG_GYRO_CONFIG, MPU6050_GYRO_FS_1000 << 3);
/* No sleep */
err |= mpu6050->dev.write(REG_PWR_MGMT1, 0);
- mpu6050->dev.sleep(250000); /* 250 ms */
+ mpu6050->dev.sleep(200000); /* 200 ms */
- shift = gyro_i16_shift(MPU6050_GYRO_FS_250);
+ shift = gyro_i16_shift(MPU6050_GYRO_FS_1000);
for (i=0; i<MPU6050_CALIBRATION_SAMPLES; i++) {
@@ -241,13 +227,63 @@ int mpu6050_calibrate_gyro(mpu6050_t *mpu6050) {
mpu6050->dev.sleep(1000); /* 1 ms */
}
- mpu6050->offset.gyro_x = -x;
- mpu6050->offset.gyro_y = -y;
- mpu6050->offset.gyro_z = -z;
+ /* the values expected for these registers are in the form 1 dps = 32.8 LSB */
+ /* the values are subtracted, so negative drift is amplified ! */
+ x = x ? -x : x;
+ y = y ? -y : y;
+ z = z ? -z : z;
+
+ /* write the values to appropriate registers */
+ err |= mpu6050->dev.write(REG_XG_OFF_USR_H, (x >> 8) & 0xFF);
+ err |= mpu6050->dev.write(REG_XG_OFF_USR_L, x & 0xFF);
+ err |= mpu6050->dev.write(REG_YG_OFF_USR_H, (y >> 8) & 0xFF);
+ err |= mpu6050->dev.write(REG_YG_OFF_USR_L, y & 0xFF);
+ err |= mpu6050->dev.write(REG_ZG_OFF_USR_H, (z >> 8) & 0xFF);
+ err |= mpu6050->dev.write(REG_ZG_OFF_USR_L, z & 0xFF);
return err;
}
+/* simply set sleep mode */
+int mpu6050_reset(mpu6050_t *mpu6050) {
+ uint8_t pwrmgmt1, sigcond;
+ int err = 0;
+
+ /* enable DEVICE_RESET */
+ err |= mpu6050->dev.write(REG_PWR_MGMT1, 0x80);
+
+ /* poll PWR_MGMT1 register until MSb is 0, which means */
+ /* the device reset process has finished. */
+ do {
+ err |= mpu6050->dev.read(REG_PWR_MGMT1, &pwrmgmt1, 1);
+ mpu6050->dev.sleep(1000); /* 1 ms */
+ } while (!err && pwrmgmt1 & 0x80);
+
+ /* enable SIG_COND_RESET */
+ err |= mpu6050->dev.write(REG_USER_CTRL, 0x01);
+
+ /* poll PWR_MGMT1 register until MSb is 0, which means */
+ /* the device reset process has finished. */
+ do {
+ err |= mpu6050->dev.read(REG_USER_CTRL, &sigcond, 1);
+ mpu6050->dev.sleep(1000); /* 1 ms */
+ } while (!err && sigcond & 0x01);
+
+ /* Overwrite old gyro offsets with 0 as they only clear on power-off */
+ err |= mpu6050->dev.write(REG_XG_OFF_USR_H, 0);
+ err |= mpu6050->dev.write(REG_XG_OFF_USR_L, 0);
+ err |= mpu6050->dev.write(REG_YG_OFF_USR_H, 0);
+ err |= mpu6050->dev.write(REG_YG_OFF_USR_L, 0);
+ err |= mpu6050->dev.write(REG_ZG_OFF_USR_H, 0);
+ err |= mpu6050->dev.write(REG_ZG_OFF_USR_L, 0);
+
+ /* enable sleep mode */
+ err |= mpu6050->dev.write(REG_PWR_MGMT1, 0x40);
+
+ 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) {
switch(fs) {
diff --git a/mpu6050.h b/mpu6050.h
index 821a1b6..6eaf227 100644
--- a/mpu6050.h
+++ b/mpu6050.h
@@ -15,7 +15,7 @@
#define MPU6050_ACC_FS_8G 0x02 /* ± 8g */
#define MPU6050_ACC_FS_16G 0x03 /* ± 16g */
-#define MPU6050_CALIBRATION_SAMPLES 1000
+#define MPU6050_CALIBRATION_SAMPLES 100
struct mpu6050_dev {
int (*init)(void);
@@ -90,5 +90,6 @@ 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);
+int mpu6050_reset(mpu6050_t *mpu6050);
#endif
\ No newline at end of file
diff --git a/registers.h b/registers.h
index 22c1e61..2a7a81e 100644
--- a/registers.h
+++ b/registers.h
@@ -1,10 +1,22 @@
#ifndef REGISTERS_H
#define REGISTERS_H
+#define REG_XA_OFF_USR_H 0x06 /* Accel X offset H */
+#define REG_XA_OFF_USR_L 0x07 /* Accel X offset L */
+#define REG_YA_OFF_USR_H 0x08 /* Accel Y offset H */
+#define REG_YA_OFF_USR_L 0x09 /* Accel Y offset L */
+#define REG_ZA_OFF_USR_H 0x0A /* Accel Z offset H */
+#define REG_ZA_OFF_USR_L 0x0B /* Accel Z offset L */
#define REG_SELF_TEST_X 0x0D
#define REG_SELF_TEST_Y 0x0E
#define REG_SELF_TEST_Z 0x0F
#define REG_SELF_TEST_A 0x10
+#define REG_XG_OFF_USR_H 0x13 /* Gyro X offset H */
+#define REG_XG_OFF_USR_L 0x14 /* Gyro X offset L */
+#define REG_YG_OFF_USR_H 0x15 /* Gyro Y offset H */
+#define REG_YG_OFF_USR_L 0x16 /* Gyro Y offset L */
+#define REG_ZG_OFF_USR_H 0x17 /* Gyro Z offset H */
+#define REG_ZG_OFF_USR_L 0x18 /* Gyro Z offset L */
#define REG_SMPLRT_DIV 0x19
#define REG_CONFIG 0x1A
#define REG_GYRO_CONFIG 0x1B