shell

main.c

@@ -8,29 +8,24 @@
 #include "i2c.h"
 
 int main() {
-
-    uint8_t id;
     uint8_t gyro, acc;
     uint8_t int_status = 0;
     uint8_t data[6];
     int16_t gx, gy, gz;
-    int16_t ax, ay, az;
     float lsb_conv_gyro;
-    float lsb_conv_acc;
     int err = 0;
+
+    mpu6050_t mpu6050;
+    mpu6050.dev.init = i2c_init;
+    mpu6050.dev.deinit = i2c_deinit;
+    mpu6050.dev.read = i2c_read;
+    mpu6050.dev.write = i2c_write;
+    mpu6050.dev.sleep = usleep;
     
-    if (i2c_init()) {
+    if (mpu6050_init(&mpu6050)) {
         exit(1);
     }
 
-    if (i2c_read(REG_WHO_AM_I, &id, 1)) {
-        exit(1);
-    }
-
-    if (id == 0x68) {
-        puts("Found MPU-6050");
-    }
-
     /* reset all signal paths */
     /* enable gyro, acc and temp */
     if (i2c_write(REG_SIGNAL_PATH_RESET, 0x07)) {
@@ -66,7 +61,6 @@ int main() {
     }
 
     acc = MPU6050_ACC_FS_2G << 3;
-    lsb_conv_acc = 16384.f;
     if (i2c_write(REG_ACCEL_CONFIG, acc)) {
         exit(1);
     }
@@ -95,23 +89,13 @@ LOOP:
     err = 0;
     int_status = 0;
 
-    do {
-        err |= i2c_read(REG_INT_STATUS, &int_status, 1);
-        usleep(1000);
-    } while(!err && !(int_status & 1));
-
-    if (i2c_read(REG_ACCEL_XOUT_H, data, 6)) {
+    if (mpu6050_read_acc(&mpu6050)) {
         exit(1);
     }
 
-    ax = (data[0] << 8 | data[1]);
-    ay = (data[2] << 8 | data[3]);
-    az = (data[4] << 8 | data[5]);
+    printf("[ACC mg] x: %-3d y: %-3d z: %-3d\n",
+        mpu6050.acc.x, mpu6050.acc.y, mpu6050.acc.z);
 
-    printf("[ACCEL g] x: % -3.4f y: %-3.4f z: %-3.4f\n",
-        (float)(ax)/lsb_conv_acc,
-        (float)(ay)/lsb_conv_acc,
-        (float)(az)/lsb_conv_acc);
     
 goto LOOP;
 

mpu6050.c

@@ -1,2 +1,69 @@
+#include <stddef.h>
+#include <string.h>
+#include <assert.h>
 #include "mpu6050.h"
-#include "registers.h"
+#include "registers.h"
+
+static uint8_t acc_i16_shift(uint8_t fs);
+
+int mpu6050_init(mpu6050_t *mpu6050) {
+    uint8_t id;
+    int err = 0;
+
+    assert(mpu6050);
+
+    if (mpu6050->dev.init != NULL) {
+        if (mpu6050->dev.init() != 0) {
+            return 1;
+        }
+    }
+
+    err |= mpu6050->dev.read(REG_WHO_AM_I, &id, 1);
+    if (id != 0x68) {
+        return 1;
+    }
+    
+    memset(&mpu6050->cfg, 0, sizeof mpu6050->cfg);
+    memset(&mpu6050->acc, 0, sizeof mpu6050->acc);
+    memset(&mpu6050->gyro, 0, sizeof mpu6050->gyro);
+
+    return err;
+}
+
+int mpu6050_deinit(mpu6050_t *mpu6050) {
+    assert(mpu6050);
+
+    if (mpu6050->dev.deinit == NULL) {
+        return 0;
+    }
+
+    return mpu6050->dev.deinit();
+}
+
+int mpu6050_read_acc(mpu6050_t *mpu6050) {
+    uint8_t data[6];
+    uint8_t shift;
+    int err = 0;
+
+    assert(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.y = (int16_t)(data[2] << 8 | data[3]) >> shift;
+    mpu6050->acc.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_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 */
+        default:                 return 0;
+    }
+}

mpu6050.h

@@ -15,26 +15,44 @@
 #define MPU6050_ACC_FS_8G    0x02 /* ± 8g */
 #define MPU6050_ACC_FS_16G   0x03 /* ± 16g */
 
-
-struct mpu6050_acc_config {
-    uint8_t range;
-    uint8_t filter;
-};
-
-struct mpu6050_gyro_config {
-    uint8_t range;
-    uint8_t filter;
+struct mpu6050_dev {
+    int (*init)(void);
+    int (*write)(uint8_t reg, uint8_t value);
+    int (*read)(uint8_t reg, uint8_t *dst, uint32_t size);
+    int (*sleep)(uint32_t dur_us);
+    int (*deinit)(void);
 };
 
 struct mpu6050_config {
-    struct mpu6050_gyro_config gyro;
-    struct mpu6050_acc_config  acc;
+    uint8_t gyro;
+    uint8_t acc;
+};
+
+/* 1 lsb = 1 mg (1/1000 g) */
+struct mpu6050_accelerometer {
+    int16_t x;
+    int16_t y;
+    int16_t z;
+};
+
+/* deg / s times 10, so 1 decimal */
+struct mpu6050_gyroscope {
+    int16_t x;
+    int16_t y;
+    int16_t z;
 };
 
 struct mpu6050 {
+    struct mpu6050_dev dev;
     struct mpu6050_config cfg;
+    struct mpu6050_accelerometer acc;
+    struct mpu6050_gyroscope gyro;
 };
 
 typedef struct mpu6050 mpu6050_t;
 
+int mpu6050_init(mpu6050_t *mpu6050);
+int mpu6050_deinit(mpu6050_t *mpu6050);
+int mpu6050_read_acc(mpu6050_t *mpu6050);
+
 #endif