compensate constant gyro drift with on-device registers
This commit is contained in:
parent
756938b190
commit
abaae7e5fa
9
main.c
9
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;
|
||||
|
||||
82
mpu6050.c
82
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) {
|
||||
|
||||
@ -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
|
||||
12
registers.h
12
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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user