The unified diff between revisions [4f22e7ef..] and [9142f333..] 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 [4f22e7ef7d3064e3b51a5b868a4722f3f13c747b] # new_revision [9142f3330490a5aa00c1686475633b620c2ef5e7] # # add_file "mpu6050.c" # content [9459051539738c013ae40e90c65e2936d93fc2bf] # ============================================================ --- /dev/null +++ mpu6050.c 9459051539738c013ae40e90c65e2936d93fc2bf @@ -0,0 +1,263 @@ +/* mpu6050.c */ + +#include "sensors.h" +#include "mpu6050.h" +#include "i2c.h" +#include "uart.h" +#include "dcm.h" +#include "fisqrt.h" +#include "stick.h" +#include "watchdog.h" +#include "status.h" +#include "abs.h" +#include "event.h" +#include "timer.h" +#include "panic.h" +#include "log.h" + +i2c_result mpu6050_result; +unsigned int mpu6050_generation; + +signed int gyro_zero_roll; +signed int gyro_zero_pitch; +signed int gyro_zero_yaw; + +#define GYRO_ZERO_COUNT 1000 + +unsigned int mpu6050_gyro_zero_count; + +unsigned char mpu6050_sample_data[14]; + +/*unsigned char mpu6050_whoami_command[1] = {0x75}; */ +unsigned char mpu6050_whoami_command[1] = {0x6B}; + +struct i2c_transaction mpu6050_whoami_transaction2; + +struct i2c_transaction mpu6050_whoami_transaction = { + (0x68 << 1) + 0, /* write */ + 1, + mpu6050_whoami_command, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + &mpu6050_whoami_transaction2 +}; + +struct i2c_transaction mpu6050_whoami_transaction2 = { + (0x68 << 1) + 1, /* read */ + 1, + mpu6050_sample_data, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + NULL +}; + +/* Accelerometer scaling */ +#define AFS_SEL 2 + + +unsigned char mpu6050_init_command[] = {0x6B, 0x01}; +unsigned char mpu6050_accel_init_command[] = {0x1c, (AFS_SEL<<3)}; + +struct i2c_transaction mpu6050_accel_init_transaction = { + (0x68 << 1) + 0, /* write */ + 2, + mpu6050_accel_init_command, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + NULL +}; + +struct i2c_transaction mpu6050_init_transaction = { + (0x68 << 1) + 0, /* write */ + 2, + mpu6050_init_command, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + &mpu6050_accel_init_transaction +}; + +unsigned char mpu6050_sample_command[] = {0x3B}; + +struct i2c_transaction mpu6050_sample_transaction2; + +struct i2c_transaction mpu6050_sample_transaction = { + (0x68 << 1) + 0, /* write */ + 1, + mpu6050_sample_command, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + &mpu6050_sample_transaction2 +}; + +struct i2c_transaction mpu6050_sample_transaction2 = { + (0x68 << 1) + 1, /* read */ + 14, + mpu6050_sample_data, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + NULL +}; + +void mpu6050_event_handler(void); + +bool mpu6050_init(void) +{ + event_register(EVENT_MPU6050_I2C_COMPLETE, mpu6050_event_handler); + + if (!i2c_start_transaction(&mpu6050_init_transaction)) + return FALSE; + while (i2c_busy()) ; + + return (mpu6050_result == I2C_SUCCESS); +} + +/* LSB / g */ +/* 1 for +- 2g */ +/* 2 for +- 4g */ +/* 4 for +- 8g */ +/* 8 for +- 16g */ +#define ACCEL_STEP (16384.0 / (float)(1<<AFS_SEL)) + +#define TWO_PI 6.28318531f +#define DEG_TO_RAD (TWO_PI/360.0f) + +/* A step of 131 = 1 degree. */ +/* Overall, this is LSB / rad/s */ +#define GYRO_STEP (131.0 / DEG_TO_RAD) + +/* LSB / degree C */ +#define TEMP_STEP 340.0 +#define TEMP_OFFSET 36.53 + +#define ACCEL_SCALE (1.0 / ACCEL_STEP) +#define GYRO_SCALE (1.0 / GYRO_STEP) +#define TEMP_SCALE (1.0 / TEMP_STEP) + +#if 0 +bool mpu6050_sample(void) +{ + unsigned int x, y, z; + unsigned int temp; + unsigned int roll, pitch, yaw; + + if (!i2c_start_transaction(&mpu6050_sample_transaction)) + return FALSE; + + while (i2c_busy()); + + if (mpu6050_result != I2C_SUCCESS) + return FALSE; + + mpu6050_result = I2C_IN_PROGRESS; + + x = (mpu6050_sample_data[0] << 8) + mpu6050_sample_data[1]; + y = (mpu6050_sample_data[2] << 8) + mpu6050_sample_data[3]; + z = (mpu6050_sample_data[4] << 8) + mpu6050_sample_data[5]; + + temp = (mpu6050_sample_data[6] << 8) + mpu6050_sample_data[7]; + + roll = (mpu6050_sample_data[ 8] << 8) + mpu6050_sample_data[ 9]; + pitch = (mpu6050_sample_data[10] << 8) + mpu6050_sample_data[11]; + yaw = (mpu6050_sample_data[12] << 8) + mpu6050_sample_data[13]; + + putstr("MPU6050 sample data:\r\n"); + putstr("x: "); + puthex(x); + putstr(", y: "); + puthex(y); + putstr(", z: "); + puthex(z); + putstr("\r\ntemperature:"); + puthex(temp); + putstr("\r\nroll:"); + puthex(roll); + putstr(", pitch:"); + puthex(pitch); + putstr(", yaw:"); + puthex(yaw); + putstr("\r\n\r\n"); +#if 0 + sensors_write_gyro_data(roll, pitch, yaw); + sensors_write_accel_data(x, y, z); +#endif + + return TRUE; +} +#endif + +bool mpu6050_start_sample(void) +{ + return i2c_start_transaction(&mpu6050_sample_transaction); +} + +void mpu6050_start_zero(void) +{ + mpu6050_gyro_zero_count = GYRO_ZERO_COUNT; + gyro_zero_roll = 0; + gyro_zero_pitch = 0; + gyro_zero_yaw = 0; +} + +void mpu6050_event_handler(void) +{ + signed short int xi, yi, zi; + signed short int tempi; + signed short int rolli, pitchi, yawi; + + float x, y, z; + float temp; + float roll, pitch, yaw; + + CHECKPOINT(9); + + if (mpu6050_result != I2C_SUCCESS) + return; + + mpu6050_result = I2C_IN_PROGRESS; + + sensors_sample_done(); + + xi = (mpu6050_sample_data[0] << 8) + mpu6050_sample_data[1]; + yi = (mpu6050_sample_data[2] << 8) + mpu6050_sample_data[3]; + zi = (mpu6050_sample_data[4] << 8) + mpu6050_sample_data[5]; + + tempi = (mpu6050_sample_data[6] << 8) + mpu6050_sample_data[7]; + + rolli = (mpu6050_sample_data[ 8] << 8)+mpu6050_sample_data[ 9]; + pitchi = (mpu6050_sample_data[10] << 8)+mpu6050_sample_data[11]; + yawi = (mpu6050_sample_data[12] << 8)+mpu6050_sample_data[13]; + + if (mpu6050_gyro_zero_count) { + gyro_zero_roll += rolli; + gyro_zero_pitch += pitchi; + gyro_zero_yaw += yawi; + if (--mpu6050_gyro_zero_count == 0) { + gyro_zero_roll /= GYRO_ZERO_COUNT; + gyro_zero_pitch /= GYRO_ZERO_COUNT; + gyro_zero_yaw /= GYRO_ZERO_COUNT; + } + } else { + rolli -= gyro_zero_roll; + pitchi -= gyro_zero_pitch; + yawi -= gyro_zero_yaw; + } + + x = ((float)xi) * ACCEL_SCALE; + y = ((float)yi) * ACCEL_SCALE; + 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; + + sensors_write_gyro_data(roll, pitch, yaw); + sensors_write_accel_data(x, y, z); + sensors_write_temp_data(temp); + + log_put_array((char *)mpu6050_sample_data, 14); + + CHECKPOINT(10); +} +