diff --git a/main.c b/main.c index 9cc5833..dc1c595 100644 --- a/main.c +++ b/main.c @@ -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; @@ -27,7 +22,7 @@ int main() { } mpu6050.cfg.gyro = MPU6050_GYRO_FS_2000; - mpu6050.cfg.acc = MPU6050_ACC_FS_16G; + mpu6050.cfg.acc = MPU6050_ACC_FS_2G; /* reset all signal paths */ /* enable gyro, acc and temp */ @@ -57,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_16G << 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(); diff --git a/mpu6050.c b/mpu6050.c index 3537f0a..85d0e91 100644 --- a/mpu6050.c +++ b/mpu6050.c @@ -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; @@ -57,6 +59,25 @@ int mpu6050_read_acc(mpu6050_t *mpu6050) { 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) { @@ -66,4 +87,15 @@ static uint8_t acc_i16_shift(uint8_t fs) { 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; + } } \ No newline at end of file diff --git a/mpu6050.h b/mpu6050.h index 11f2e64..7d193bd 100644 --- a/mpu6050.h +++ b/mpu6050.h @@ -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 \ No newline at end of file