main.c

@@ -8,9 +8,14 @@
 #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;
@@ -21,9 +26,6 @@ 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)) {
@@ -52,33 +54,50 @@ int main() {
         exit(1);
     }
 
-    if (i2c_write(REG_GYRO_CONFIG, mpu6050.cfg.gyro << 3)) {
+    gyro = MPU6050_GYRO_FS_250 << 3;
+    lsb_conv_gyro = 131.f;
+    if (i2c_write(REG_GYRO_CONFIG, gyro)) {
         exit(1);
     }
 
-    if (i2c_write(REG_ACCEL_CONFIG, mpu6050.cfg.acc << 3)) {
+    acc = MPU6050_ACC_FS_2G << 3;
+    if (i2c_write(REG_ACCEL_CONFIG, acc)) {
         exit(1);
     }
 
     usleep(200 * 1000);
 
 LOOP:
-    if (mpu6050_read_gyro(&mpu6050)) {
+    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)) {
         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);
 
-    usleep(100 * 1000);
+    
-    goto LOOP;
+goto LOOP;
 
     i2c_deinit();
 

mpu6050.c

@@ -5,7 +5,6 @@
 #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;
@@ -41,7 +40,6 @@ 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;
@@ -52,50 +50,20 @@ 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 4; /* 16384 LSb / g */
+        case MPU6050_ACC_FS_2G:  return 2; /* 16384 LSb / g */
         case MPU6050_ACC_FS_4G:  return 3; /* 8192 LSb / g */
-        case MPU6050_ACC_FS_8G:  return 2; /* 4096 LSb / g */
+        case MPU6050_ACC_FS_8G:  return 4; /* 4096 LSb / g */
-        case MPU6050_ACC_FS_16G: return 1; /* 2048 LSb / g */
+        case MPU6050_ACC_FS_16G: return 5; /* 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;
-    }
 }

mpu6050.h

@@ -35,7 +35,7 @@ struct mpu6050_accelerometer {
     int16_t z;
 };
 
-/* 1 lsb = 0.1 deg / s */
+/* deg / s times 10, so 1 decimal */
 struct mpu6050_gyroscope {
     int16_t x;
     int16_t y;
@@ -54,6 +54,5 @@ 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