Bye bye Wii Motion Plus, hello MPU6050. Also, increase control loop
to 200Hz, add SD card logging, and a number of other changes.
This commit is contained in:
263
mpu6050.c
Normal file
263
mpu6050.c
Normal file
@@ -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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user