william/mpu6050
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

gyro

Browse Source
This commit is contained in:
William Clark 2024-01-10 02:16:07 +00:00
parent b7be8b093f
commit 160ca4efe5
3 changed files with 44 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
main.c
View File

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

32
mpu6050.c
View File

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

3
mpu6050.h
View File

@ -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