The unified diff between revisions [056a532c..] and [dc88787e..] is displayed below. It can also be downloaded as a raw diff.
This diff has been restricted to the following files: 'mpu6050.c'
# # old_revision [056a532c92301bcb224e1f786c5f6720e8acf3eb] # new_revision [dc88787ecd1d574feba045763baed2a7651ff33d] # # patch "mpu6050.c" # from [9459051539738c013ae40e90c65e2936d93fc2bf] # to [9ef021ddd54f35cd39874f1538d4ac9b76b69ec5] # ============================================================ --- mpu6050.c 9459051539738c013ae40e90c65e2936d93fc2bf +++ mpu6050.c 9ef021ddd54f35cd39874f1538d4ac9b76b69ec5 @@ -14,6 +14,7 @@ #include "timer.h" #include "panic.h" #include "log.h" +#include "config.h" i2c_result mpu6050_result; unsigned int mpu6050_generation; @@ -51,29 +52,49 @@ struct i2c_transaction mpu6050_whoami_tr NULL }; -/* Accelerometer scaling */ -#define AFS_SEL 2 +#define FS_SEL config.mpu6050.gyro_sensitivity +#define AFS_SEL config.mpu6050.accel_sensitivity unsigned char mpu6050_init_command[] = {0x6B, 0x01}; -unsigned char mpu6050_accel_init_command[] = {0x1c, (AFS_SEL<<3)}; +unsigned char mpu6050_gyro_init_command[] = {0x1b, 0 /* (FS_SEL<<3) to be filled in at runtime */}; +unsigned char mpu6050_accel_init_command[] = {0x1c, 0 /* (AFS_SEL<<3) to be filled in at runtime */}; +unsigned char mpu6050_bypass_init_command[] = {0x37, 0x02}; +struct i2c_transaction mpu6050_bypass_init_transaction = { + (0x68 << 1) + 0, /* write */ + 2, + mpu6050_bypass_init_command, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + NULL +}; + struct i2c_transaction mpu6050_accel_init_transaction = { (0x68 << 1) + 0, /* write */ 2, mpu6050_accel_init_command, &mpu6050_result, EVENT_MPU6050_I2C_COMPLETE, - NULL + &mpu6050_bypass_init_transaction }; +struct i2c_transaction mpu6050_gyro_init_transaction = { + (0x68 << 1) + 0, /* write */ + 2, + mpu6050_gyro_init_command, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + &mpu6050_accel_init_transaction +}; + struct i2c_transaction mpu6050_init_transaction = { (0x68 << 1) + 0, /* write */ 2, mpu6050_init_command, &mpu6050_result, EVENT_MPU6050_I2C_COMPLETE, - &mpu6050_accel_init_transaction + &mpu6050_gyro_init_transaction }; unsigned char mpu6050_sample_command[] = {0x3B}; @@ -104,6 +125,10 @@ bool mpu6050_init(void) { event_register(EVENT_MPU6050_I2C_COMPLETE, mpu6050_event_handler); + /* Fill in sensitivity accordingly, as it's not known at compile time */ + mpu6050_gyro_init_command[1] = FS_SEL << 3; + mpu6050_accel_init_command[1] = AFS_SEL << 3; + if (!i2c_start_transaction(&mpu6050_init_transaction)) return FALSE; while (i2c_busy()) ; @@ -123,7 +148,7 @@ bool mpu6050_init(void) /* A step of 131 = 1 degree. */ /* Overall, this is LSB / rad/s */ -#define GYRO_STEP (131.0 / DEG_TO_RAD) +#define GYRO_STEP (131.0 / DEG_TO_RAD / (float)(1<<FS_SEL)) /* LSB / degree C */ #define TEMP_STEP 340.0 @@ -204,9 +229,8 @@ void mpu6050_event_handler(void) signed short int tempi; signed short int rolli, pitchi, yawi; - float x, y, z; + vec3f accel, gyro; float temp; - float roll, pitch, yaw; CHECKPOINT(9); @@ -242,18 +266,18 @@ void mpu6050_event_handler(void) yawi -= gyro_zero_yaw; } - x = ((float)xi) * ACCEL_SCALE; - y = ((float)yi) * ACCEL_SCALE; - z = ((float)zi) * ACCEL_SCALE; + accel.x = ((float)xi) * ACCEL_SCALE; + accel.y = ((float)yi) * ACCEL_SCALE; + accel.z = ((float)zi) * ACCEL_SCALE; temp = ((float)tempi) * TEMP_SCALE + TEMP_OFFSET; - roll = ((float)rolli) * GYRO_SCALE; - pitch = ((float)pitchi) * GYRO_SCALE; - yaw = ((float)yawi) * GYRO_SCALE; + gyro.x = ((float)rolli) * GYRO_SCALE; + gyro.y = ((float)pitchi) * GYRO_SCALE; + gyro.z = ((float)yawi) * GYRO_SCALE; - sensors_write_gyro_data(roll, pitch, yaw); - sensors_write_accel_data(x, y, z); + sensors_write_gyro_data(gyro); + sensors_write_accel_data(accel); sensors_write_temp_data(temp); log_put_array((char *)mpu6050_sample_data, 14);