gyro

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;
@@ -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();
 

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;
@@ -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;
+    }
+}

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