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

configure()

Browse Source
This commit is contained in:
William Clark 2024-01-10 20:09:23 +00:00
parent e8615511d9
commit c2930c3020
3 changed files with 76 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
main.c
View File

@ -23,68 +23,34 @@ int main() {
mpu6050.cfg.gyro = MPU6050_GYRO_FS_250;
mpu6050.cfg.acc = MPU6050_ACC_FS_2G;
mpu6050.cfg.dlpl = 6;
mpu6050.cfg.sdiv = 200;
mpu6050.cfg.int_enable.data_rdy = 1;
/* reset all signal paths */
/* enable gyro, acc and temp */
if (i2c_write(REG_SIGNAL_PATH_RESET, 0x07)) {
if (mpu6050_configure(&mpu6050)) {
exit(1);
}
/* set sample rate divisor to 150 */
/* if LP-filter = 0 or 7 => sample rate = 8 Khz, otherwise 1 Khz */
if (i2c_write(REG_SMPLRT_DIV, 150)) {
exit(1);
for ( ;; ) {
if (mpu6050_read(&mpu6050)) {
exit(1);
}
printf("[GYRO °/s] x:%4.1f y:%4.1f z:%4.1f ",
(float)mpu6050.data.gyro.x / 10.f,
(float)mpu6050.data.gyro.y / 10.f,
(float)mpu6050.data.gyro.z / 10.f);
printf("[ACC g] x:%4.3f y:%4.3f z:%4.3f ",
(float)mpu6050.data.acc.x / 1000.f,
(float)mpu6050.data.acc.y / 1000.f,
(float)mpu6050.data.acc.z / 1000.f);
printf("t:%4.1f°C\n", (float)mpu6050.data.temp / 10.f);
}
/* set up digital LP-filter */
if (i2c_write(REG_CONFIG, 0x05)) {
exit(1);
}
/* set sleep to 0 */
if (i2c_write(REG_PWR_MGMT1, 0x00)) {
exit(1);
}
/* enable DATA_RDY interrupt (polling for now) */
/* active when internal writes to all out data regs are done */
if (i2c_write(REG_INT_ENABLE, mpu6050.cfg.int_enable.data_rdy & 1)) {
exit(1);
}
if (i2c_write(REG_GYRO_CONFIG, mpu6050.cfg.gyro << 3)) {
exit(1);
}
if (i2c_write(REG_ACCEL_CONFIG, mpu6050.cfg.acc << 3)) {
exit(1);
}
usleep(200 * 1000);
LOOP:
if (mpu6050_read(&mpu6050)) {
exit(1);
}
printf("[GYRO °/s] x:%4.1f y:%4.1f z:%4.1f ",
(float)mpu6050.data.gyro.x / 10.f,
(float)mpu6050.data.gyro.y / 10.f,
(float)mpu6050.data.gyro.z / 10.f);
printf("[ACC g] x:%4.3f y:%4.3f z:%4.3f ",
(float)mpu6050.data.acc.x / 1000.f,
(float)mpu6050.data.acc.y / 1000.f,
(float)mpu6050.data.acc.z / 1000.f);
printf("t:%4.1f°C\n", (float)mpu6050.data.temp / 10.f);
usleep(100 * 1000);
goto LOOP;
i2c_deinit();
mpu6050_deinit(&mpu6050);
return 0;
}

58
mpu6050.c
View File

@ -86,9 +86,9 @@ int mpu6050_read_gyro(mpu6050_t *mpu6050) {
err |= mpu6050->dev.read(REG_GYRO_XOUT_H, data, 6);
mpu6050->data.gyro.x = ((int16_t)(data[0] << 8 | data[1]) >> shift);
mpu6050->data.gyro.y = ((int16_t)(data[2] << 8 | data[3]) >> shift);
mpu6050->data.gyro.z = ((int16_t)(data[4] << 8 | data[5]) >> shift);
mpu6050->data.gyro.x = (int16_t)(data[0] << 8 | data[1]) >> shift;
mpu6050->data.gyro.y = (int16_t)(data[2] << 8 | data[3]) >> shift;
mpu6050->data.gyro.z = (int16_t)(data[4] << 8 | data[5]) >> shift;
return err;
}
@ -133,13 +133,59 @@ int mpu6050_read(mpu6050_t *mpu6050) {
/* 6-7 temp */
mpu6050->data.temp = (int16_t)(data[6] << 8 | data[7])/34 + 365;
/* 8-13 gyro */
mpu6050->data.gyro.x = ((int16_t)(data[ 8] << 8 | data[ 9]) >> shift_gyro);
mpu6050->data.gyro.y = ((int16_t)(data[10] << 8 | data[11]) >> shift_gyro);
mpu6050->data.gyro.z = ((int16_t)(data[12] << 8 | data[13]) >> shift_gyro);
mpu6050->data.gyro.x = (int16_t)(data[8] << 8 | data[9]) >> shift_gyro;
mpu6050->data.gyro.y = (int16_t)(data[10] << 8 | data[11]) >> shift_gyro;
mpu6050->data.gyro.z = (int16_t)(data[12] << 8 | data[13]) >> shift_gyro;
return err;
}
/* write configuration to device */
/* then sleep for 200 ms */
int mpu6050_configure(mpu6050_t *mpu6050) {
uint8_t sig_path, dlpl, sleep, inten;
uint8_t acc, gyro;
int err = 0;
assert(mpu6050);
/* enable accel, gyro and temp */
sig_path = 0x07;
/* digital low-pass filter level */
dlpl = mpu6050->cfg.dlpl & 0x07;
/* INT_ENABLE */
inten = mpu6050->cfg.int_enable.data_rdy & 1;
inten |= (mpu6050->cfg.int_enable.i2c_mst & 1) << 3;
inten |= (mpu6050->cfg.int_enable.fifo_overflow & 1) << 4;
inten |= (mpu6050->cfg.int_enable.mot & 1) << 6;
/* PWR_MGMT1 */
sleep = 0;
/* gyro */
gyro = (mpu6050->cfg.gyro & 0x02) << 3;
/* accelerometer */
acc = (mpu6050->cfg.acc & 0x02) << 3;
err |= mpu6050->dev.write(REG_SMPLRT_DIV, mpu6050->cfg.sdiv);
err |= mpu6050->dev.write(REG_SIGNAL_PATH_RESET, sig_path);
err |= mpu6050->dev.write(REG_INT_ENABLE, inten);
err |= mpu6050->dev.write(REG_CONFIG, dlpl);
err |= mpu6050->dev.write(REG_ACCEL_CONFIG, acc);
err |= mpu6050->dev.write(REG_GYRO_CONFIG, gyro);
err |= mpu6050->dev.write(REG_PWR_MGMT1, sleep);
if (!err) {
mpu6050->dev.sleep(200000); /* 200 ms */
}
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) {

3
mpu6050.h
View File

@ -35,6 +35,8 @@ struct mpu6050_int_enable {
struct mpu6050_config {
uint8_t gyro;
uint8_t acc;
uint8_t dlpl; /* digital low-pass filter level [0-7]*/
uint8_t sdiv; /* sample rate divider. ~ lpl; lpl=(0,7) => divides 8KHz else 1 KHz*/
struct mpu6050_int_enable int_enable;
};
@ -73,5 +75,6 @@ int mpu6050_read_acc(mpu6050_t *mpu6050);
int mpu6050_read_gyro(mpu6050_t *mpu6050);
int mpu6050_read_temp(mpu6050_t *mpu6050);
int mpu6050_read(mpu6050_t *mpu6050);
int mpu6050_configure(mpu6050_t *mpu6050);
#endif