From a627f657d80e7673365e391d185e6a2c8b8f8cce Mon Sep 17 00:00:00 2001 From: William Clark Date: Wed, 10 Jan 2024 00:33:31 +0000 Subject: [PATCH] shell --- main.c | 38 +++++++++--------------------- mpu6050.c | 69 ++++++++++++++++++++++++++++++++++++++++++++++++++++++- mpu6050.h | 40 +++++++++++++++++++++++--------- 3 files changed, 108 insertions(+), 39 deletions(-) diff --git a/main.c b/main.c index d4e7714..30af7d2 100644 --- a/main.c +++ b/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; diff --git a/mpu6050.c b/mpu6050.c index e860762..c09bb9d 100644 --- a/mpu6050.c +++ b/mpu6050.c @@ -1,2 +1,69 @@ +#include +#include +#include #include "mpu6050.h" -#include "registers.h" \ No newline at end of file +#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; + } +} \ No newline at end of file diff --git a/mpu6050.h b/mpu6050.h index ad36432..11f2e64 100644 --- a/mpu6050.h +++ b/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 \ No newline at end of file