Compare commits

...

2 Commits

Author SHA1 Message Date
160ca4efe5 gyro 2024-01-10 02:16:07 +00:00
b7be8b093f fix wrong acc_i16_shift values 2024-01-10 01:03:54 +00:00
3 changed files with 50 additions and 36 deletions

43
main.c
View File

@ -8,14 +8,9 @@
#include "i2c.h"
int main() {
uint8_t gyro, acc;
uint8_t int_status = 0;
uint8_t data[6];
int16_t gx, gy, gz;
float lsb_conv_gyro;
int err = 0;
mpu6050_t mpu6050;
mpu6050.dev.init = i2c_init;
mpu6050.dev.deinit = i2c_deinit;
mpu6050.dev.read = i2c_read;
@ -26,6 +21,9 @@ int main() {
exit(1);
}
mpu6050.cfg.gyro = MPU6050_GYRO_FS_2000;
mpu6050.cfg.acc = MPU6050_ACC_FS_2G;
/* reset all signal paths */
/* enable gyro, acc and temp */
if (i2c_write(REG_SIGNAL_PATH_RESET, 0x07)) {
@ -54,50 +52,33 @@ int main() {
exit(1);
}
gyro = MPU6050_GYRO_FS_250 << 3;
lsb_conv_gyro = 131.f;
if (i2c_write(REG_GYRO_CONFIG, gyro)) {
if (i2c_write(REG_GYRO_CONFIG, mpu6050.cfg.gyro << 3)) {
exit(1);
}
acc = MPU6050_ACC_FS_2G << 3;
if (i2c_write(REG_ACCEL_CONFIG, acc)) {
if (i2c_write(REG_ACCEL_CONFIG, mpu6050.cfg.acc << 3)) {
exit(1);
}
usleep(200 * 1000);
LOOP:
do {
err |= i2c_read(REG_INT_STATUS, &int_status, 1);
usleep(1000);
} while(!err && !(int_status & 1));
if (i2c_read(REG_GYRO_XOUT_H, data, 6)) {
if (mpu6050_read_gyro(&mpu6050)) {
exit(1);
}
gx = (data[0] << 8 | data[1]);
gy = (data[2] << 8 | data[3]);
gz = (data[4] << 8 | data[5]);
printf("[GYRO °/s] x: % -3.4f y: %-3.4f z: %-3.4f ",
(float)(gx)/lsb_conv_gyro,
(float)(gy)/lsb_conv_gyro,
(float)(gz)/lsb_conv_gyro);
err = 0;
int_status = 0;
if (mpu6050_read_acc(&mpu6050)) {
exit(1);
}
printf("[GYRO °/s] x:%1.d y:%1.d z:%1.d ",
mpu6050.gyro.x, mpu6050.gyro.y, mpu6050.gyro.z);
printf("[ACC mg] x: %-3d y: %-3d z: %-3d\n",
mpu6050.acc.x, mpu6050.acc.y, mpu6050.acc.z);
goto LOOP;
usleep(100 * 1000);
goto LOOP;
i2c_deinit();

View File

@ -5,6 +5,7 @@
#include "registers.h"
static uint8_t acc_i16_shift(uint8_t fs);
static uint8_t gyro_i16_shift(uint8_t fs);
int mpu6050_init(mpu6050_t *mpu6050) {
uint8_t id;
@ -40,6 +41,7 @@ int mpu6050_deinit(mpu6050_t *mpu6050) {
return mpu6050->dev.deinit();
}
/* transform i16 samples st 1 lsb is 1 mg (1/1000 g) */
int mpu6050_read_acc(mpu6050_t *mpu6050) {
uint8_t data[6];
uint8_t shift;
@ -50,20 +52,50 @@ int mpu6050_read_acc(mpu6050_t *mpu6050) {
shift = acc_i16_shift(mpu6050->cfg.acc);
err |= mpu6050->dev.read(REG_ACCEL_XOUT_H, data, 6);
mpu6050->acc.x = ((int16_t)(data[0] << 8 | data[1])) >> shift;
mpu6050->acc.x = (int16_t)(data[0] << 8 | data[1]) >> shift;
mpu6050->acc.y = (int16_t)(data[2] << 8 | data[3]) >> shift;
mpu6050->acc.z = (int16_t)(data[4] << 8 | data[5]) >> shift;
return err;
}
/* transform gyro i16 sample st 1 lsb = 0.1 deg / s */
int mpu6050_read_gyro(mpu6050_t *mpu6050) {
uint8_t data[6];
uint8_t shift;
int err = 0;
assert(mpu6050);
shift = gyro_i16_shift(mpu6050->cfg.gyro);
err |= mpu6050->dev.read(REG_GYRO_XOUT_H, data, 6);
mpu6050->gyro.x = ((int16_t)(data[0] << 8 | data[1]) >> shift);
mpu6050->gyro.y = ((int16_t)(data[2] << 8 | data[3]) >> shift);
mpu6050->gyro.z = ((int16_t)(data[4] << 8 | data[5]) >> shift);
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) {
case MPU6050_ACC_FS_2G: return 2; /* 16384 LSb / g */
case MPU6050_ACC_FS_2G: return 4; /* 16384 LSb / g */
case MPU6050_ACC_FS_4G: return 3; /* 8192 LSb / g */
case MPU6050_ACC_FS_8G: return 4; /* 4096 LSb / g */
case MPU6050_ACC_FS_16G: return 5; /* 2048 LSb / g */
case MPU6050_ACC_FS_8G: return 2; /* 4096 LSb / g */
case MPU6050_ACC_FS_16G: return 1; /* 2048 LSb / g */
default: return 0;
}
}
/* How much to shift down an i16 sample depending on full-scale mode set */
static uint8_t gyro_i16_shift(uint8_t fs) {
switch(fs) {
case MPU6050_GYRO_FS_250: return 4;
case MPU6050_GYRO_FS_500: return 3;
case MPU6050_GYRO_FS_1000: return 2;
case MPU6050_GYRO_FS_2000: return 1;
default: return 0;
}
}

View File

@ -35,7 +35,7 @@ struct mpu6050_accelerometer {
int16_t z;
};
/* deg / s times 10, so 1 decimal */
/* 1 lsb = 0.1 deg / s */
struct mpu6050_gyroscope {
int16_t x;
int16_t y;
@ -54,5 +54,6 @@ typedef struct mpu6050 mpu6050_t;
int mpu6050_init(mpu6050_t *mpu6050);
int mpu6050_deinit(mpu6050_t *mpu6050);
int mpu6050_read_acc(mpu6050_t *mpu6050);
int mpu6050_read_gyro(mpu6050_t *mpu6050);
#endif