Quadrotor from scratch
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

264 lines
5.7 KiB

/* 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);
}