25 changed files with 1724 additions and 580 deletions
@ -0,0 +1,110 @@ |
|||||||
|
/* log.c */ |
||||||
|
|
||||||
|
#include "types.h" |
||||||
|
#include "sdcard.h" |
||||||
|
#include "uart.h" |
||||||
|
#include "timer.h" |
||||||
|
#include "log.h" |
||||||
|
|
||||||
|
/* This is shared with sdcard.c */ |
||||||
|
bool log_enabled; |
||||||
|
|
||||||
|
char log_buffer[LOG_BUFFERSIZE]; |
||||||
|
unsigned int log_bufferstart; |
||||||
|
unsigned int log_bufferend; |
||||||
|
|
||||||
|
unsigned int log_generation; |
||||||
|
|
||||||
|
/* DO NOT call when the buffer is empty */ |
||||||
|
/* This should be safe against writes to the buffer, as the writes only
|
||||||
|
* affect log_bufferend. So no blocking of interrupts is necessary. |
||||||
|
*/ |
||||||
|
char log_get_byte(void) |
||||||
|
{ |
||||||
|
char i; |
||||||
|
i = log_buffer[log_bufferstart++]; |
||||||
|
log_bufferstart = log_bufferstart % LOG_BUFFERSIZE; |
||||||
|
|
||||||
|
return i; |
||||||
|
} |
||||||
|
|
||||||
|
void log_put_byte(char c) |
||||||
|
{ |
||||||
|
if (!log_enabled) |
||||||
|
return; |
||||||
|
|
||||||
|
/* If the buffer is full, we just discard data.
|
||||||
|
* Better than overrunning. |
||||||
|
*/ |
||||||
|
if (((log_bufferend + 1) % LOG_BUFFERSIZE) == log_bufferstart) |
||||||
|
return; |
||||||
|
log_buffer[log_bufferend++] = c; |
||||||
|
log_bufferend = log_bufferend % LOG_BUFFERSIZE; |
||||||
|
#if 0 |
||||||
|
putint(c); |
||||||
|
putch(' '); |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
void log_put_uint16(unsigned int i) |
||||||
|
{ |
||||||
|
log_put_byte(i & 0xff); |
||||||
|
log_put_byte((i >> 8) & 0xff); |
||||||
|
} |
||||||
|
|
||||||
|
void log_put_uint(unsigned int i) |
||||||
|
{ |
||||||
|
log_put_byte(i & 0xff); |
||||||
|
log_put_byte((i >> 8) & 0xff); |
||||||
|
log_put_byte((i >> 16) & 0xff); |
||||||
|
log_put_byte((i >> 24) & 0xff); |
||||||
|
} |
||||||
|
|
||||||
|
void log_put_header(unsigned int timestamp) |
||||||
|
{ |
||||||
|
log_put_uint(LOG_MAGIC); |
||||||
|
log_put_uint(log_generation); |
||||||
|
log_put_uint(timestamp); |
||||||
|
log_put_uint(log_read_busytime()); |
||||||
|
} |
||||||
|
|
||||||
|
void log_put_array(char *data, int length) |
||||||
|
{ |
||||||
|
int i; |
||||||
|
|
||||||
|
for (i = 0; i < length; i++) |
||||||
|
log_put_byte(data[i]); |
||||||
|
} |
||||||
|
|
||||||
|
void log_put_float(float f) |
||||||
|
{ |
||||||
|
union { |
||||||
|
float f; |
||||||
|
unsigned int i; |
||||||
|
} data; |
||||||
|
data.f = f; |
||||||
|
log_put_uint(data.i); |
||||||
|
} |
||||||
|
|
||||||
|
unsigned int log_busystamp; |
||||||
|
unsigned int log_busytime; |
||||||
|
|
||||||
|
void log_mark_busy(void) |
||||||
|
{ |
||||||
|
unsigned int time = timer_read(); |
||||||
|
log_busystamp = time; |
||||||
|
} |
||||||
|
|
||||||
|
void log_mark_idle(void) |
||||||
|
{ |
||||||
|
unsigned int time = timer_read(); |
||||||
|
unsigned int diff = time - log_busystamp; |
||||||
|
log_busytime += diff; |
||||||
|
} |
||||||
|
|
||||||
|
unsigned int log_read_busytime(void) |
||||||
|
{ |
||||||
|
unsigned int time = log_busytime; |
||||||
|
log_busytime = 0; |
||||||
|
return time; |
||||||
|
} |
||||||
@ -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); |
||||||
|
} |
||||||
|
|
||||||
@ -0,0 +1,678 @@ |
|||||||
|
/* sdcard.c */ |
||||||
|
|
||||||
|
#include "spi.h" |
||||||
|
#include "types.h" |
||||||
|
#include "uart.h" |
||||||
|
#include "timer.h" |
||||||
|
#include "event.h" |
||||||
|
#include "log.h" |
||||||
|
|
||||||
|
#define spi_write_array(x) spi_write_bytes(x, sizeof(x)/sizeof(x[0])) |
||||||
|
|
||||||
|
#define SDCARD_COMMAND_TIMEOUT 0xffff |
||||||
|
|
||||||
|
char dummy_block[] = {0xff, 0xff, 0xff, 0xff, 0xff, |
||||||
|
0xff, 0xff, 0xff, 0xff, 0xff}; |
||||||
|
char reset_command[] = {0x40, 0, 0, 0, 0, 0x95}; |
||||||
|
|
||||||
|
/* Voltage = 2.7-3.6V, check pattern = 0xaa, CRC matters for CMD8 */ |
||||||
|
char sdcard_cmd8[] = {0x48, 0, 0, 0x01, 0xaa, 0x87}; |
||||||
|
|
||||||
|
char sdcard_cmd55[] = {0x77, 0, 0, 0, 0, 0xff}; |
||||||
|
|
||||||
|
char sdcard_acmd41[] = {0x69, 0, 0, 0, 0, 0xff}; |
||||||
|
char sdcard_acmd41_hcs[] = {0x69, 0x40, 0, 0, 0, 0xff}; |
||||||
|
|
||||||
|
char sdcard_cmd58[] = {0x7a, 0, 0, 0, 0, 0xff}; |
||||||
|
|
||||||
|
/* 512 bytes block length */ |
||||||
|
char sdcard_cmd16[] = {0x50, 0, 0, 2, 0, 0xff}; |
||||||
|
|
||||||
|
/* Read CSD */ |
||||||
|
char sdcard_cmd9[] = {0x49, 0, 0, 0, 0, 0xff}; |
||||||
|
|
||||||
|
|
||||||
|
static bool high_capacity; |
||||||
|
|
||||||
|
#ifdef SDCARD_BOUNDARY_128K |
||||||
|
/* 128K */ |
||||||
|
#define SDCARD_BOUNDARY_MASK 0xff |
||||||
|
#define SDCARD_BOUNDARY_SIZE 0x100 |
||||||
|
#else |
||||||
|
/* 32K */ |
||||||
|
#define SDCARD_BOUNDARY_MASK 0x3f |
||||||
|
#define SDCARD_BOUNDARY_SIZE 0x40 |
||||||
|
#endif |
||||||
|
|
||||||
|
unsigned int sdcard_sector; |
||||||
|
unsigned int sdcard_offset; |
||||||
|
unsigned int sdcard_size; /* defined as number of sectors */ |
||||||
|
|
||||||
|
#define SDCARD_IDLE 0 |
||||||
|
#define SDCARD_WRITE_GAP 1 |
||||||
|
#define SDCARD_WRITING_BLOCK 2 |
||||||
|
#define SDCARD_STOPPING 3 |
||||||
|
#define SDCARD_ERROR 4 |
||||||
|
|
||||||
|
unsigned int sdcard_active; |
||||||
|
|
||||||
|
static bool sdcard_command(char *command, unsigned int command_length, |
||||||
|
char *response, unsigned int response_length, bool wait_busy); |
||||||
|
|
||||||
|
bool sdcard_write(unsigned int address, char *buffer, unsigned int length); |
||||||
|
bool sdcard_read_csd(char *buffer); |
||||||
|
void sdcard_prepare(void); |
||||||
|
|
||||||
|
/* SD card (SPI mode initialisation)
|
||||||
|
|
||||||
|
power on |
||||||
|
|
||||||
|
CMD0+ |
||||||
|
CMD8 |
||||||
|
if (no response from CMD8) { |
||||||
|
// legacy (MMC) card
|
||||||
|
CMD58 (optional, read OCR) - no or bad response = don't use card |
||||||
|
while (ACMD41(arg 0x00) & in_idle_state_mask) |
||||||
|
; |
||||||
|
done |
||||||
|
} else { |
||||||
|
// SD card
|
||||||
|
if (response from CMD8 was present but invalid |
||||||
|
(check pattern not matched)) |
||||||
|
retry CMD8; |
||||||
|
CMD58 (optional, read OCR) |
||||||
|
while (ACMD41(arg HCS=1) & in_idle_state_mask) |
||||||
|
; |
||||||
|
CMD58 (Get CCS) |
||||||
|
if (CCS) |
||||||
|
done - high capacity SD card |
||||||
|
else |
||||||
|
done - standard SD card |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
*/ |
||||||
|
|
||||||
|
bool init_sdcard(void) |
||||||
|
{ |
||||||
|
char response[16]; |
||||||
|
unsigned int i; |
||||||
|
|
||||||
|
unsigned int read_bl_len, c_size_mult, c_size; |
||||||
|
unsigned int block_len, mult, blocknr; |
||||||
|
|
||||||
|
putstr("Initialising SPI\r\n"); |
||||||
|
|
||||||
|
init_spi(); |
||||||
|
|
||||||
|
high_capacity = FALSE; |
||||||
|
|
||||||
|
putstr("Sending 80 clocks\r\n"); |
||||||
|
|
||||||
|
spi_transaction_start(); |
||||||
|
spi_write_array(dummy_block); |
||||||
|
spi_transaction_stop(); |
||||||
|
|
||||||
|
putstr("Sending reset command\r\n"); |
||||||
|
|
||||||
|
if (!sdcard_command(reset_command, sizeof(reset_command), |
||||||
|
response, 1, FALSE)) |
||||||
|
return FALSE; |
||||||
|
|
||||||
|
putstr("Reset command successful. Checking response.\r\n"); |
||||||
|
|
||||||
|
puthex(response[0]); |
||||||
|
|
||||||
|
putstr("\r\n"); |
||||||
|
|
||||||
|
if (response[0] != 0x01) |
||||||
|
return FALSE; |
||||||
|
|
||||||
|
putstr("Sending CMD8\r\n"); |
||||||
|
|
||||||
|
if (!sdcard_command(sdcard_cmd8, sizeof(sdcard_cmd8), |
||||||
|
response, 5, FALSE)) |
||||||
|
{ |
||||||
|
putstr("No response. Legacy device.\r\n"); |
||||||
|
/* Legacy device */ |
||||||
|
do { |
||||||
|
if (!sdcard_command(sdcard_cmd55, sizeof(sdcard_cmd55), |
||||||
|
response, 1, FALSE)) |
||||||
|
return FALSE; |
||||||
|
if (response[0] != 0x01) |
||||||
|
return FALSE; |
||||||
|
if (!sdcard_command(sdcard_acmd41, |
||||||
|
sizeof(sdcard_acmd41), |
||||||
|
response, 1, FALSE)) |
||||||
|
return FALSE; |
||||||
|
} while (response[0] & 1); |
||||||
|
putstr("ACMD41 gave us the right response.\r\n"); |
||||||
|
} else { |
||||||
|
putstr("We got a response. Not a legacy device.\r\n"); |
||||||
|
/* Not legacy device */ |
||||||
|
for (i = 1; i < 4; i++) { |
||||||
|
if (response[i] != sdcard_cmd8[i]) { |
||||||
|
/* We should really retry here. Meh. */ |
||||||
|
return FALSE; |
||||||
|
} |
||||||
|
} |
||||||
|
putstr("Response OK. Safe to continue.\r\n"); |
||||||
|
do { |
||||||
|
if (!sdcard_command(sdcard_cmd55, sizeof(sdcard_cmd55), |
||||||
|
response, 1, FALSE)) |
||||||
|
return FALSE; |
||||||
|
if (response[0] != 0x01) |
||||||
|
return FALSE; |
||||||
|
if (!sdcard_command(sdcard_acmd41_hcs, |
||||||
|
sizeof(sdcard_acmd41_hcs), |
||||||
|
response, 1, FALSE)) |
||||||
|
return FALSE; |
||||||
|
} while (response[0] & 1); |
||||||
|
putstr("ACMD41 gave us the right response.\r\n"); |
||||||
|
if (!sdcard_command(sdcard_cmd58, sizeof(sdcard_cmd58), |
||||||
|
response, 5, FALSE)) |
||||||
|
return FALSE; |
||||||
|
putstr("OCR register retrieved.\r\n"); |
||||||
|
if ((response[1] & 0x80) == 0) |
||||||
|
return FALSE; |
||||||
|
putstr("Chip isn't still powering up.\r\n"); |
||||||
|
if (response[1] & 0x40) { |
||||||
|
putstr("We have a high capacity device.\r\n"); |
||||||
|
high_capacity = TRUE; |
||||||
|
} else { |
||||||
|
putstr("We have a low capacity device.\r\n"); |
||||||
|
high_capacity = FALSE; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
spi_speedup(); |
||||||
|
|
||||||
|
/* Set block length to 512 */ |
||||||
|
if (!sdcard_command(sdcard_cmd16, sizeof(sdcard_cmd16), |
||||||
|
response, 1, FALSE)) |
||||||
|
return FALSE; |
||||||
|
|
||||||
|
putstr("Determining card size.\r\n"); |
||||||
|
|
||||||
|
if (!sdcard_read_csd(response)) |
||||||
|
return FALSE; |
||||||
|
|
||||||
|
putstr("Read CSD\r\n"); |
||||||
|
|
||||||
|
switch ((response[0] & 0xc0) >> 6) { |
||||||
|
case 0: |
||||||
|
/* CSD Version 1.0 */ |
||||||
|
read_bl_len = response[5] & 0x0f; |
||||||
|
c_size_mult = ((response[9] & 0x03) << 1) | (response[10] >> 7); |
||||||
|
c_size = ((response[6] & 0x03) << 10) | (response[7] << 2) | |
||||||
|
(response[8] >> 6); |
||||||
|
|
||||||
|
block_len = 1<<read_bl_len; |
||||||
|
mult = 1<<(c_size_mult+2); |
||||||
|
blocknr = (c_size+1) * mult; |
||||||
|
sdcard_size = blocknr * block_len / 512; |
||||||
|
break; |
||||||
|
case 1: |
||||||
|
/* CSD Version 2.0 */ |
||||||
|
c_size = ((response[7] & 0x3f) << 16) | |
||||||
|
(response[8] << 8) | response[9]; |
||||||
|
sdcard_size = (c_size+1) * 1024; |
||||||
|
break; |
||||||
|
default: |
||||||
|
/* Unrecognised CSD version */ |
||||||
|
putstr("Unrecognised CSD version\r\n"); |
||||||
|
return FALSE; |
||||||
|
} |
||||||
|
|
||||||
|
putstr("SD initialisation sequence complete.\r\n"); |
||||||
|
putstr("size = "); |
||||||
|
putint(sdcard_size / 2); |
||||||
|
putstr("KB\r\n"); |
||||||
|
|
||||||
|
putstr("Initialising logging system.\r\n"); |
||||||
|
sdcard_prepare(); |
||||||
|
|
||||||
|
return TRUE; |
||||||
|
} |
||||||
|
|
||||||
|
static bool sdcard_command_innards(char *command, unsigned int command_length, |
||||||
|
char *response, unsigned int response_length, bool wait_busy) |
||||||
|
{ |
||||||
|
char byte; |
||||||
|
unsigned int i; |
||||||
|
|
||||||
|
spi_write_bytes(command, command_length); |
||||||
|
|
||||||
|
i = 0; |
||||||
|
|
||||||
|
do |
||||||
|
{ |
||||||
|
byte = spi_read_byte(); |
||||||
|
i++; |
||||||
|
} while (((byte & 0x80) != 0) && (i < SDCARD_COMMAND_TIMEOUT)); |
||||||
|
|
||||||
|
if (byte & 0x80) |
||||||
|
return FALSE; |
||||||
|
|
||||||
|
if (response_length > 0) |
||||||
|
response[0] = byte; |
||||||
|
|
||||||
|
/* We need to store the response, plus read one extra byte for luck. */ |
||||||
|
/* XXX not an extra byte for luck any more */ |
||||||
|
for (i = 1; i < response_length; i++) { |
||||||
|
byte = spi_read_byte(); |
||||||
|
response[i] = byte; |
||||||
|
} |
||||||
|
|
||||||
|
if (wait_busy) { |
||||||
|
do { |
||||||
|
byte = spi_read_byte(); |
||||||
|
} while (byte == 0); |
||||||
|
|
||||||
|
spi_write_byte(0xff); |
||||||
|
} |
||||||
|
|
||||||
|
return TRUE; |
||||||
|
} |
||||||
|
|
||||||
|
static bool sdcard_check_data_response(void) |
||||||
|
{ |
||||||
|
char byte; |
||||||
|
unsigned int i; |
||||||
|
|
||||||
|
i = 0; |
||||||
|
|
||||||
|
do |
||||||
|
{ |
||||||
|
byte = spi_read_byte(); |
||||||
|
i++; |
||||||
|
} while (((byte & 0x11) != 0x01) && (i < SDCARD_COMMAND_TIMEOUT)); |
||||||
|
|
||||||
|
if ((byte & 0x11) != 0x01) |
||||||
|
return FALSE; |
||||||
|
|
||||||
|
if ((byte & 0x0f) != 0x05) /* Data accepted */ |
||||||
|
return FALSE; |
||||||
|
|
||||||
|
/* Read one more byte for luck */ |
||||||
|
byte = spi_read_byte(); |
||||||
|
|
||||||
|
return TRUE; |
||||||
|
} |
||||||
|
|
||||||
|
static bool sdcard_command(char *command, unsigned int command_length, |
||||||
|
char *response, unsigned int response_length, bool wait_busy) |
||||||
|
{ |
||||||
|
bool result; |
||||||
|
|
||||||
|
spi_transaction_start(); |
||||||
|
|
||||||
|
result = sdcard_command_innards(command, command_length, |
||||||
|
response, response_length, wait_busy); |
||||||
|
|
||||||
|
spi_transaction_stop(); |
||||||
|
|
||||||
|
return result; |
||||||
|
} |
||||||
|
|
||||||
|
static bool sdcard_read_block(char *buffer, unsigned int length) |
||||||
|
{ |
||||||
|
unsigned int i; |
||||||
|
unsigned int crc_hi; |
||||||
|
unsigned int crc_lo; |
||||||
|
unsigned int crc; |
||||||
|
|
||||||
|
while (1) { |
||||||
|
char byte = spi_read_byte(); |
||||||
|
if (byte == 0xff) |
||||||
|
continue; |
||||||
|
if (byte == 0xfe) |
||||||
|
break; |
||||||
|
if ((byte & 0xf0) == 0) |
||||||
|
if (byte != 0) |
||||||
|
return FALSE; |
||||||
|
} |
||||||
|
|
||||||
|
/* We need to store the response, plus read one extra byte for luck. */ |
||||||
|
for (i = 0; i < length; i++) { |
||||||
|
buffer[i] = spi_read_byte(); |
||||||
|
} |
||||||
|
|
||||||
|
crc_hi = spi_read_byte(); |
||||||
|
crc_lo = spi_read_byte(); |
||||||
|
|
||||||
|
crc = (crc_hi << 8) + crc_lo; |
||||||
|
|
||||||
|
/* XXX check CRC and return FALSE if doesn't match */ |
||||||
|
|
||||||
|
return TRUE; |
||||||
|
} |
||||||
|
|
||||||
|
bool sdcard_read(unsigned int address, char *buffer, unsigned int length) |
||||||
|
{ |
||||||
|
bool valid; |
||||||
|
char response; |
||||||
|
|
||||||
|
char cmd[6]; |
||||||
|
|
||||||
|
if (!high_capacity) |
||||||
|
address = address * 512; |
||||||
|
|
||||||
|
cmd[0] = 0x51; /* CMD17 */ |
||||||
|
cmd[1] = (address >> 24) & 0xff; |
||||||
|
cmd[2] = (address >> 16) & 0xff; |
||||||
|
cmd[3] = (address >> 8) & 0xff; |
||||||
|
cmd[4] = (address >> 0) & 0xff; |
||||||
|
cmd[5] = 0xff; /* dummy CRC */ |
||||||
|
|
||||||
|
spi_transaction_start(); |
||||||
|
|
||||||
|
if (!sdcard_command_innards(cmd, sizeof(cmd), |
||||||
|
&response, 1, FALSE)) { |
||||||
|
spi_transaction_stop(); |
||||||
|
return FALSE; |
||||||
|
} |
||||||
|
|
||||||
|
if (response != 0) { |
||||||
|
spi_transaction_stop(); |
||||||
|
return FALSE; |
||||||
|
} |
||||||
|
|
||||||
|
valid = sdcard_read_block(buffer, length); |
||||||
|
|
||||||
|
spi_transaction_stop(); |
||||||
|
|
||||||
|
return valid; |
||||||
|
} |
||||||
|
|
||||||
|
bool sdcard_read_csd(char *buffer) |
||||||
|
{ |
||||||
|
bool valid; |
||||||
|
char response; |
||||||
|
|
||||||
|
spi_transaction_start(); |
||||||
|
|
||||||
|
if (!sdcard_command_innards(sdcard_cmd9, sizeof(sdcard_cmd9), |
||||||
|
&response, 1, FALSE)) { |
||||||
|
spi_transaction_stop(); |
||||||
|
return FALSE; |
||||||
|
} |
||||||
|
|
||||||
|
if (response != 0) { |
||||||
|
spi_transaction_stop(); |
||||||
|
return FALSE; |
||||||
|
} |
||||||
|
|
||||||
|
valid = sdcard_read_block(buffer, 16); |
||||||
|
|
||||||
|
spi_transaction_stop(); |
||||||
|
|
||||||
|
return valid; |
||||||
|
} |
||||||
|
|
||||||
|
bool sdcard_send_write_cmd(unsigned int address) |
||||||
|
{ |
||||||
|
char response; |
||||||
|
|
||||||
|
char cmd[6]; |
||||||
|
|
||||||
|
if (!high_capacity) |
||||||
|
address = address * 512; |
||||||
|
|
||||||
|
cmd[0] = 0x59; /* CMD25 */ |
||||||
|
cmd[1] = (address >> 24) & 0xff; |
||||||
|
cmd[2] = (address >> 16) & 0xff; |
||||||
|
cmd[3] = (address >> 8) & 0xff; |
||||||
|
cmd[4] = (address >> 0) & 0xff; |
||||||
|
cmd[5] = 0xff; /* dummy CRC */ |
||||||
|
|
||||||
|
spi_transaction_start(); |
||||||
|
|
||||||
|
if (!sdcard_command_innards(cmd, sizeof(cmd), |
||||||
|
&response, 1, FALSE)) { |
||||||
|
spi_transaction_stop(); |
||||||
|
return FALSE; |
||||||
|
} |
||||||
|
|
||||||
|
if (response != 0) { |
||||||
|
spi_transaction_stop(); |
||||||
|
return FALSE; |
||||||
|
} |
||||||
|
|
||||||
|
return TRUE; |
||||||
|
} |
||||||
|
|
||||||
|
static void sdcard_send_data_token(void) |
||||||
|
{ |
||||||
|
spi_write_byte(0xfc); |
||||||
|
} |
||||||
|
|
||||||
|
static void sdcard_send_stop_token(void) |
||||||
|
{ |
||||||
|
spi_write_byte(0xfd); |
||||||
|
} |
||||||
|
|
||||||
|
#define READ_UINT(b, i) ((b)[(i)] + ((b)[(i)+1] << 8) + \ |
||||||
|
((b)[(i)+2] << 16) + ((b)[(i)+3] << 24)) |
||||||
|
|
||||||
|
#define WRITE_UINT(b, i, d) \ |
||||||
|
do { \
|
||||||
|
(b)[(i)] = (d) & 0xff; \
|
||||||
|
(b)[(i)+1] = ((d) >> 8) & 0xff; \
|
||||||
|
(b)[(i)+2] = ((d) >> 16) & 0xff; \
|
||||||
|
(b)[(i)+3] = ((d) >> 24) & 0xff; \
|
||||||
|
} while (0) |
||||||
|
|
||||||
|
|
||||||
|
/* We assume that the magic is to be found within this area. If not,
|
||||||
|
* we will need to read a bigger area. If the typical record size grows |
||||||
|
* to more than a sector, for example, then we will need to read in multiple |
||||||
|
* sectors where this function is called. |
||||||
|
*/ |
||||||
|
bool sdcard_scan_magic(char *buffer, unsigned int size, unsigned int generation) |
||||||
|
{ |
||||||
|
unsigned int i; |
||||||
|
|
||||||
|
for (i = 0; i < size - 8; i++) { |
||||||
|
if ((buffer[i] == (LOG_MAGIC & 0xff)) && |
||||||
|
(buffer[i+1] == ((LOG_MAGIC >> 8) & 0xff)) && |
||||||
|
(buffer[i+2] == ((LOG_MAGIC >> 16) & 0xff)) && |
||||||
|
(buffer[i+3] == ((LOG_MAGIC >> 24) & 0xff)) && |
||||||
|
(buffer[i+4] == ((generation >> 0) & 0xff)) && |
||||||
|
(buffer[i+5] == ((generation >> 8) & 0xff)) && |
||||||
|
(buffer[i+6] == ((generation >> 16) & 0xff)) && |
||||||
|
(buffer[i+7] == ((generation >> 24) & 0xff))) |
||||||
|
return TRUE; |
||||||
|
} |
||||||
|
|
||||||
|
return FALSE; |
||||||
|
} |
||||||
|
|
||||||
|
void sdcard_prepare(void) |
||||||
|
{ |
||||||
|
unsigned int magic; |
||||||
|
unsigned int start_sector; |
||||||
|
unsigned int count; |
||||||
|
|
||||||
|
if (!sdcard_read(0, log_buffer, 512)) |
||||||
|
return; |
||||||
|
|
||||||
|
magic = READ_UINT(log_buffer, 0); |
||||||
|
|
||||||
|
if (magic != LOG_MAGIC) { |
||||||
|
unsigned int i; |
||||||
|
for (i = 0; i < 512; i++) |
||||||
|
log_buffer[i] = 0; |
||||||
|
WRITE_UINT(log_buffer, 0, LOG_MAGIC); |
||||||
|
start_sector = SDCARD_BOUNDARY_SIZE; |
||||||
|
log_generation = 0; |
||||||
|
putstr("Did not find header. Formatting.\r\n"); |
||||||
|
} else { |
||||||
|
start_sector = READ_UINT(log_buffer, 4); |
||||||
|
log_generation = READ_UINT(log_buffer, 8); |
||||||
|
count = 0; |
||||||
|
putstr("Found header.\r\n"); |
||||||
|
putstr("Last started at sector "); |
||||||
|
putint(start_sector); |
||||||
|
putstr(" with generation "); |
||||||
|
putint(log_generation); |
||||||
|
putstr("\r\n"); |
||||||
|
while (1) { |
||||||
|
if (!sdcard_read(start_sector, log_buffer+512, 512)) |
||||||
|
return; |
||||||
|
/* This needs to change if record length exceeds 512 */ |
||||||
|
if (sdcard_scan_magic(log_buffer+512, 512, |
||||||
|
log_generation)) { |
||||||
|
start_sector += SDCARD_BOUNDARY_SIZE; |
||||||
|
if (start_sector >= sdcard_size) |
||||||
|
start_sector = SDCARD_BOUNDARY_SIZE; |
||||||
|
} else { |
||||||
|
break; |
||||||
|
} |
||||||
|
if (count++ > (sdcard_size / SDCARD_BOUNDARY_SIZE)) { |
||||||
|
start_sector = SDCARD_BOUNDARY_SIZE; |
||||||
|
break; |
||||||
|
} |
||||||
|
} |
||||||
|
log_generation++; |
||||||
|
} |
||||||
|
|
||||||
|
WRITE_UINT(log_buffer, 4, start_sector); |
||||||
|
WRITE_UINT(log_buffer, 8, log_generation); |
||||||
|
|
||||||
|
putstr("Starting at sector "); |
||||||
|
putint(start_sector); |
||||||
|
putstr(" with generation "); |
||||||
|
putint(log_generation); |
||||||
|
putstr("\r\n"); |
||||||
|
|
||||||
|
if (!sdcard_write(0, log_buffer, 512)) |
||||||
|
return; |
||||||
|
|
||||||
|
sdcard_sector = start_sector; |
||||||
|
sdcard_offset = 0; |
||||||
|
log_enabled = TRUE; |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
static bool sdcard_busy(void) |
||||||
|
{ |
||||||
|
return (spi_read_byte() != 0xff); |
||||||
|
} |
||||||
|
|
||||||
|
static void sdcard_send_dummy_crc(void) |
||||||
|
{ |
||||||
|
spi_write_byte(0xff); |
||||||
|
spi_write_byte(0xff); |
||||||
|
} |
||||||
|
|
||||||
|
void sdcard_poll(void) |
||||||
|
{ |
||||||
|
if (!log_enabled) |
||||||
|
return; |
||||||
|
if (LOG_BUFFER_EMPTY) |
||||||
|
return; |
||||||
|
log_mark_busy(); |
||||||
|
if (sdcard_active == SDCARD_IDLE) { |
||||||
|
spi_transaction_start(); |
||||||
|
if (sdcard_busy()) { |
||||||
|
spi_transaction_stop(); |
||||||
|
log_mark_idle(); |
||||||
|
return; |
||||||
|
} |
||||||
|
putch('C'); |
||||||
|
if (sdcard_send_write_cmd(sdcard_sector)) |
||||||
|
sdcard_active = SDCARD_WRITE_GAP; |
||||||
|
else { |
||||||
|
spi_transaction_stop(); |
||||||
|
sdcard_active = SDCARD_ERROR; |
||||||
|
} |
||||||
|
} |
||||||
|
if (sdcard_active == SDCARD_WRITE_GAP) { |
||||||
|
if (sdcard_busy()) { |
||||||
|
log_mark_idle(); |
||||||
|
return; |
||||||
|
} |
||||||
|
sdcard_send_data_token(); |
||||||
|
sdcard_active = SDCARD_WRITING_BLOCK; |
||||||
|
} |
||||||
|
if (sdcard_active == SDCARD_WRITING_BLOCK) { |
||||||
|
unsigned int bytes_to_end_of_sector; |
||||||
|
unsigned int i; |
||||||
|
|
||||||
|
i = LOG_BUFFER_BYTES; |
||||||
|
bytes_to_end_of_sector = 512 - sdcard_offset; |
||||||
|
if (i > bytes_to_end_of_sector) |
||||||
|
i = bytes_to_end_of_sector; |
||||||
|
if (i > 32) |
||||||
|
i = 32; |
||||||
|
sdcard_offset += i; |
||||||
|
while (i--) { |
||||||
|
spi_write_byte(log_get_byte()); |
||||||
|
} |
||||||
|
if (sdcard_offset >= 512) { |
||||||
|
sdcard_offset = 0; |
||||||
|
sdcard_sector++; |
||||||
|
sdcard_send_dummy_crc(); |
||||||
|
putch('.'); |
||||||
|
if (!sdcard_check_data_response()) { |
||||||
|
/* Set state to STOPPING instead? */ |
||||||
|
/* How do we test this? */ |
||||||
|
spi_transaction_stop(); |
||||||
|
sdcard_active = SDCARD_ERROR; |
||||||
|
log_mark_idle(); |
||||||
|
return; |
||||||
|
} |
||||||
|
sdcard_active = SDCARD_WRITE_GAP; |
||||||
|
if ((sdcard_sector & SDCARD_BOUNDARY_MASK) == 0) { |
||||||
|
putch('S'); |
||||||
|
sdcard_active = SDCARD_STOPPING; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
if (sdcard_active == SDCARD_STOPPING) { |
||||||
|
if (sdcard_busy()) { |
||||||
|
log_mark_idle(); |
||||||
|
return; |
||||||
|
} |
||||||
|
sdcard_send_stop_token(); |
||||||
|
spi_transaction_stop(); |
||||||
|
sdcard_active = SDCARD_IDLE; |
||||||
|
} |
||||||
|
log_mark_idle(); |
||||||
|
} |
||||||
|
|
||||||
|
bool sdcard_write(unsigned int address, char *buffer, unsigned int length) |
||||||
|
{ |
||||||
|
unsigned int i; |
||||||
|
|
||||||
|
spi_transaction_start(); |
||||||
|
|
||||||
|
if (!sdcard_send_write_cmd(address)) { |
||||||
|
spi_transaction_stop(); |
||||||
|
return FALSE; |
||||||
|
} |
||||||
|
|
||||||
|
sdcard_send_data_token(); |
||||||
|
|
||||||
|
for (i = 0; i < length; i++) { |
||||||
|
spi_write_byte(buffer[i]); |
||||||
|
} |
||||||
|
|
||||||
|
sdcard_send_dummy_crc(); |
||||||
|
if (!sdcard_check_data_response()) { |
||||||
|
spi_transaction_stop(); |
||||||
|
return FALSE; |
||||||
|
} |
||||||
|
|
||||||
|
while (sdcard_busy()) ; |
||||||
|
|
||||||
|
sdcard_send_stop_token(); |
||||||
|
|
||||||
|
while (sdcard_busy()) ; |
||||||
|
|
||||||
|
spi_transaction_stop(); |
||||||
|
|
||||||
|
return TRUE; |
||||||
|
} |
||||||
|
|
||||||
@ -0,0 +1,233 @@ |
|||||||
|
/* sensors.c */ |
||||||
|
|
||||||
|
#include "mpu6050.h" |
||||||
|
#include "dcm.h" |
||||||
|
#include "fisqrt.h" |
||||||
|
#include "watchdog.h" |
||||||
|
#include "status.h" |
||||||
|
#include "abs.h" |
||||||
|
#include "panic.h" |
||||||
|
#include "uart.h" |
||||||
|
#include "log.h" |
||||||
|
#include "stick.h" |
||||||
|
|
||||||
|
bool (*sensor_start_fns[])(void) = { |
||||||
|
mpu6050_start_sample, |
||||||
|
}; |
||||||
|
|
||||||
|
#define SENSOR_START_FNS (sizeof(sensor_start_fns)/sizeof(sensor_start_fns[0])) |
||||||
|
|
||||||
|
static unsigned int next_sensor; |
||||||
|
|
||||||
|
static bool sensors_zero; |
||||||
|
static bool sensors_update; |
||||||
|
static unsigned int sensors_discard; |
||||||
|
static unsigned int sensors_generation; |
||||||
|
|
||||||
|
float sensors_gyro_roll; |
||||||
|
float sensors_gyro_pitch; |
||||||
|
float sensors_gyro_yaw; |
||||||
|
|
||||||
|
float sensors_temp; |
||||||
|
|
||||||
|
float sensors_accel_x; |
||||||
|
float sensors_accel_y; |
||||||
|
float sensors_accel_z; |
||||||
|
|
||||||
|
float gyro_yaw_zero; |
||||||
|
float gyro_pitch_zero; |
||||||
|
float gyro_roll_zero; |
||||||
|
|
||||||
|
void sensors_write_log(void); |
||||||
|
void sensors_process(void); |
||||||
|
|
||||||
|
#define TWO_PI 6.28318531f |
||||||
|
#define DEG_TO_RAD (TWO_PI/360.0f) |
||||||
|
|
||||||
|
/* The gyro has to stay within this limit in each axis in order to arm */ |
||||||
|
#define GYRO_RATE_THRESHOLD (0.01 / DEG_TO_RAD) |
||||||
|
|
||||||
|
#define GYRO_ZERO_COUNT 1000 |
||||||
|
|
||||||
|
void sensors_dump(void); |
||||||
|
|
||||||
|
bool sensors_init(void) |
||||||
|
{ |
||||||
|
next_sensor = 0; |
||||||
|
|
||||||
|
if (!mpu6050_init()) |
||||||
|
return FALSE; |
||||||
|
|
||||||
|
return TRUE; |
||||||
|
} |
||||||
|
|
||||||
|
bool sensors_next_sample(void) |
||||||
|
{ |
||||||
|
bool result; |
||||||
|
|
||||||
|
result = (sensor_start_fns[next_sensor])(); |
||||||
|
if (next_sensor >= SENSOR_START_FNS) |
||||||
|
next_sensor = 0; |
||||||
|
|
||||||
|
return result; |
||||||
|
} |
||||||
|
|
||||||
|
void sensors_sample_done(void) |
||||||
|
{ |
||||||
|
if (next_sensor == 0) { |
||||||
|
sensors_write_log(); |
||||||
|
return; |
||||||
|
} |
||||||
|
|
||||||
|
if (!sensors_next_sample()) |
||||||
|
panic(PANIC_SENSOR_FAIL); |
||||||
|
} |
||||||
|
|
||||||
|
bool sensors_start_sample(void) |
||||||
|
{ |
||||||
|
next_sensor = 0; |
||||||
|
return sensors_next_sample(); |
||||||
|
} |
||||||
|
|
||||||
|
void sensors_write_gyro_data(float roll, float pitch, float yaw) |
||||||
|
{ |
||||||
|
#if 0 |
||||||
|
sensors_gyro_roll = roll - gyro_roll_zero; |
||||||
|
sensors_gyro_pitch = pitch - gyro_pitch_zero; |
||||||
|
sensors_gyro_yaw = yaw - gyro_yaw_zero; |
||||||
|
#else |
||||||
|
sensors_gyro_roll = roll; |
||||||
|
sensors_gyro_pitch = pitch; |
||||||
|
sensors_gyro_yaw = yaw; |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
void sensors_write_accel_data(float x, float y, float z) |
||||||
|
{ |
||||||
|
sensors_accel_x = x; |
||||||
|
sensors_accel_y = y; |
||||||
|
sensors_accel_z = z; |
||||||
|
} |
||||||
|
|
||||||
|
void sensors_write_temp_data(float temp) |
||||||
|
{ |
||||||
|
sensors_temp = temp; |
||||||
|
/* XXX HACK find a better place for this call */ |
||||||
|
sensors_process(); |
||||||
|
} |
||||||
|
|
||||||
|
#define LOG_SIGNATURE_SENSORS 0xDA7ADA7A |
||||||
|
#define LOG_SIGNATURE_SENSORS2 0xDA7AF173 |
||||||
|
void sensors_write_log(void) |
||||||
|
{ |
||||||
|
#if 0 |
||||||
|
log_put_uint(LOG_SIGNATURE_SENSORS); |
||||||
|
log_put_float(sensors_accel_x); |
||||||
|
log_put_float(sensors_accel_y); |
||||||
|
log_put_float(sensors_accel_z); |
||||||
|
log_put_float(sensors_gyro_roll); |
||||||
|
log_put_float(sensors_gyro_pitch); |
||||||
|
log_put_float(sensors_gyro_yaw); |
||||||
|
log_put_float(sensors_temp); |
||||||
|
#else |
||||||
|
/* XXX this just about comes out in the right place, but by luck */ |
||||||
|
log_put_uint(LOG_SIGNATURE_SENSORS2); |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
void sensors_start_zero(void) |
||||||
|
{ |
||||||
|
sensors_zero = TRUE; |
||||||
|
sensors_update = FALSE; |
||||||
|
sensors_discard = 100; |
||||||
|
sensors_generation = 0; |
||||||
|
putstr("Starting zero\r\n"); |
||||||
|
mpu6050_start_zero(); |
||||||
|
} |
||||||
|
|
||||||
|
void sensors_process(void) |
||||||
|
{ |
||||||
|
if (sensors_update) { |
||||||
|
#if 1 |
||||||
|
dcm_update(-sensors_gyro_pitch, -sensors_gyro_roll, |
||||||
|
-sensors_gyro_yaw); |
||||||
|
#else |
||||||
|
dcm_update(0.0, 0.0, 0.0); |
||||||
|
#endif |
||||||
|
if (!status_armed()) { |
||||||
|
if ( (abs(sensors_gyro_roll) < GYRO_RATE_THRESHOLD) && |
||||||
|
(abs(sensors_gyro_pitch) < GYRO_RATE_THRESHOLD) && |
||||||
|
(abs(sensors_gyro_yaw) < GYRO_RATE_THRESHOLD)) { |
||||||
|
status_set_ready(STATUS_MODULE_GYRO_RATE, TRUE); |
||||||
|
} else { |
||||||
|
status_set_ready(STATUS_MODULE_GYRO_RATE, FALSE); |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
|
|
||||||
|
sensors_generation++; |
||||||
|
|
||||||
|
#if SEND_DCM |
||||||
|
if ((sensors_generation % 40) == 0) { |
||||||
|
dcm_send_packet(); |
||||||
|
sensors_dump(); |
||||||
|
} |
||||||
|
#endif |
||||||
|
|
||||||
|
} else if (sensors_zero) { |
||||||
|
if (sensors_discard) { |
||||||
|
sensors_discard--; |
||||||
|
} else { |
||||||
|
gyro_yaw_zero += sensors_gyro_yaw; |
||||||
|
gyro_pitch_zero += sensors_gyro_pitch; |
||||||
|
gyro_roll_zero += sensors_gyro_roll; |
||||||
|
sensors_generation++; |
||||||
|
if (sensors_generation >= GYRO_ZERO_COUNT) { |
||||||
|
sensors_zero = FALSE; |
||||||
|
sensors_update = TRUE; |
||||||
|
sensors_generation = 0; |
||||||
|
gyro_yaw_zero /= GYRO_ZERO_COUNT; |
||||||
|
gyro_pitch_zero /= GYRO_ZERO_COUNT; |
||||||
|
gyro_roll_zero /= GYRO_ZERO_COUNT; |
||||||
|
putstr("Zero finished\r\n"); |
||||||
|
status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE); |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
watchdog_kick(WATCHDOG_GYRO); |
||||||
|
|
||||||
|
#if 1 |
||||||
|
dcm_drift_correction(-sensors_accel_y, -sensors_accel_x, |
||||||
|
-sensors_accel_z); |
||||||
|
#endif |
||||||
|
#if 0 |
||||||
|
dcm_drift_correction(sensors_accel_x, sensors_accel_y, |
||||||
|
sensors_accel_z); |
||||||
|
#endif |
||||||
|
|
||||||
|
watchdog_kick(WATCHDOG_ACCEL); |
||||||
|
stick_input(); |
||||||
|
} |
||||||
|
|
||||||
|
void sensors_dump(void) |
||||||
|
{ |
||||||
|
putstr("("); |
||||||
|
putint_s((int)(sensors_accel_x * 1000.0)); |
||||||
|
putstr(","); |
||||||
|
putint_s((int)(sensors_accel_y * 1000.0)); |
||||||
|
putstr(","); |
||||||
|
putint_s((int)(sensors_accel_z * 1000.0)); |
||||||
|
putstr(")"); |
||||||
|
|
||||||
|
putstr("("); |
||||||
|
putint_s((int)(sensors_gyro_roll * 1000.0)); |
||||||
|
putstr(","); |
||||||
|
putint_s((int)(sensors_gyro_pitch * 1000.0)); |
||||||
|
putstr(","); |
||||||
|
putint_s((int)(sensors_gyro_yaw * 1000.0)); |
||||||
|
putstr(")"); |
||||||
|
|
||||||
|
putstr("("); |
||||||
|
putint_s((int)(sensors_temp * 1000.0)); |
||||||
|
putstr(")\r\n"); |
||||||
|
} |
||||||
@ -0,0 +1,116 @@ |
|||||||
|
/* spi.c */ |
||||||
|
|
||||||
|
#include "spi.h" |
||||||
|
#include "interrupt.h" |
||||||
|
#include "event.h" |
||||||
|
#include "uart.h" |
||||||
|
|
||||||
|
#define SSPBASE 0xE0068000 |
||||||
|
|
||||||
|
#define SSPCR0 0x00 |
||||||
|
#define SSPCR1 0x04 |
||||||
|
#define SSPDR 0x08 |
||||||
|
#define SSPSR 0x0c |
||||||
|
#define SSPCPSR 0x10 |
||||||
|
#define SSPIMSC 0x14 |
||||||
|
#define SSPRIS 0x18 |
||||||
|
#define SSPMIS 0x1c |
||||||
|
#define SSPICR 0x20 |
||||||
|
|
||||||
|
#define REG(x) (((volatile unsigned char *)SSPBASE)[x]) |
||||||
|
#define WREG(x) (((volatile unsigned int *)SSPBASE)[(x)/sizeof(unsigned int)]) |
||||||
|
|
||||||
|
#define TNF (REG(SSPSR) & (1<<1)) |
||||||
|
#define RNE (REG(SSPSR) & (1<<2)) |
||||||
|
|
||||||
|
#define FP0XVAL (*((volatile unsigned int *) 0x3FFFC014)) |
||||||
|
#define FP0XSET (*((volatile unsigned int *) 0x3FFFC018)) |
||||||
|
#define FP0XCLR (*((volatile unsigned int *) 0x3FFFC01C)) |
||||||
|
|
||||||
|
void init_spi(void) |
||||||
|
{ |
||||||
|
WREG(SSPCR0) = 0x1f07; /* SPI clock = PCLK/64, mode 0, 8 bits */ |
||||||
|
// WREG(SSPCR0) = 0xff07;
|
||||||
|
/* Set to 0x0007 later */ |
||||||
|
REG(SSPCPSR) = 2; /* Divide PCLK by 2 */ |
||||||
|
REG(SSPCR1) = 0x0002; /* Enable SSP, Master mode */ |
||||||
|
} |
||||||
|
|
||||||
|
void spi_speedup(void) |
||||||
|
{ |
||||||
|
#if 1 |
||||||
|
WREG(SSPCR0) = 0x0107; /* SPI clock = PCLK/4, mode 0, 8 bits */ |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
void spi_write_byte(char byte) |
||||||
|
{ |
||||||
|
unsigned int dummy; |
||||||
|
|
||||||
|
while (!TNF) ; |
||||||
|
WREG(SSPDR) = byte; |
||||||
|
|
||||||
|
while (!RNE) ; |
||||||
|
dummy = REG(SSPDR); |
||||||
|
|
||||||
|
#ifdef SPIDEBUG |
||||||
|
putstr(">"); |
||||||
|
puthex(byte); |
||||||
|
putstr("("); |
||||||
|
puthex(dummy); |
||||||
|
putstr(") "); |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
char spi_read_byte(void) |
||||||
|
{ |
||||||
|
char byte; |
||||||
|
|
||||||
|
while (!TNF) ; |
||||||
|
WREG(SSPDR) = 0xff; |
||||||
|
|
||||||
|
while (!RNE) ; |
||||||
|
byte = (char) REG(SSPDR); |
||||||
|
|
||||||
|
#ifdef SPIDEBUG |
||||||
|
putstr("<"); |
||||||
|
puthex(byte); |
||||||
|
putstr(" "); |
||||||
|
#endif |
||||||
|
|
||||||
|
return byte; |
||||||
|
} |
||||||
|
|
||||||
|
void spi_write_bytes(char *data, int len) |
||||||
|
{ |
||||||
|
while (len--) |
||||||
|
spi_write_byte(*data++); |
||||||
|
} |
||||||
|
|
||||||
|
void spi_read_bytes(char *data, int len) |
||||||
|
{ |
||||||
|
while (len--) |
||||||
|
*data++ = spi_read_byte(); |
||||||
|
} |
||||||
|
|
||||||
|
void spi_transaction_start(void) |
||||||
|
{ |
||||||
|
FP0XCLR = 0x00200000; |
||||||
|
} |
||||||
|
|
||||||
|
void spi_transaction_stop(void) |
||||||
|
{ |
||||||
|
FP0XSET = 0x00200000; |
||||||
|
} |
||||||
|
|
||||||
|
void spi_drain(void) |
||||||
|
{ |
||||||
|
char byte; |
||||||
|
putstr("Draining:"); |
||||||
|
while (RNE) { |
||||||
|
byte = (char) REG(SSPDR); |
||||||
|
putstr(" "); |
||||||
|
puthex(byte); |
||||||
|
} |
||||||
|
putstr("\r\n"); |
||||||
|
} |
||||||
@ -1,320 +0,0 @@ |
|||||||
/* wmp.c */ |
|
||||||
|
|
||||||
#include "wmp.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" |
|
||||||
|
|
||||||
#define WMP_ZERO_COUNT 100 |
|
||||||
|
|
||||||
|
|
||||||
#define ACCEL_ZERO_X 520 |
|
||||||
#define ACCEL_ZERO_Y 516 |
|
||||||
#define ACCEL_ZERO_Z 514 |
|
||||||
|
|
||||||
/*
|
|
||||||
516, 510, 710 |
|
||||||
-4, -6, 196 |
|
||||||
16, 36, 38416 = 38468 |
|
||||||
sqrt(38468) = 196.1326... |
|
||||||
... somehow once we scale by gravity we get almost exactly 0.05. |
|
||||||
*/ |
|
||||||
|
|
||||||
#define ACCEL_SCALE 0.05 |
|
||||||
|
|
||||||
/* Nunchuck pass-through mode */ |
|
||||||
unsigned char wmp_init_command[2] = {0xfe, 0x05}; |
|
||||||
|
|
||||||
i2c_result wmp_result; |
|
||||||
unsigned int wmp_generation; |
|
||||||
|
|
||||||
struct i2c_transaction wmp_init_transaction = { |
|
||||||
(0x53 << 1) + 0, /* write */ |
|
||||||
2, |
|
||||||
wmp_init_command, |
|
||||||
&wmp_result, |
|
||||||
NULL |
|
||||||
}; |
|
||||||
|
|
||||||
unsigned char wmp_read_cal_command[1] = {0x20}; |
|
||||||
|
|
||||||
struct i2c_transaction wmp_read_cal_transaction2; |
|
||||||
|
|
||||||
struct i2c_transaction wmp_read_cal_transaction = { |
|
||||||
(0x53 << 1) + 0, /* write */ |
|
||||||
1, |
|
||||||
wmp_read_cal_command, |
|
||||||
&wmp_result, |
|
||||||
&wmp_read_cal_transaction2 |
|
||||||
}; |
|
||||||
|
|
||||||
struct i2c_transaction wmp_read_cal_transaction2 = { |
|
||||||
(0x53 << 1) + 1, /* read */ |
|
||||||
0x20, |
|
||||||
wmp_calibration_data, |
|
||||||
&wmp_result, |
|
||||||
NULL |
|
||||||
}; |
|
||||||
|
|
||||||
unsigned char wmp_sample_command[1] = {0x00}; |
|
||||||
|
|
||||||
unsigned char wmp_sample_data[6]; |
|
||||||
|
|
||||||
struct i2c_transaction wmp_sample_transaction2; |
|
||||||
|
|
||||||
struct i2c_transaction wmp_sample_transaction = { |
|
||||||
(0x52 << 1) + 0, /* write */ |
|
||||||
1, |
|
||||||
wmp_sample_command, |
|
||||||
&wmp_result, |
|
||||||
&wmp_sample_transaction2 |
|
||||||
}; |
|
||||||
|
|
||||||
struct i2c_transaction wmp_sample_transaction2 = { |
|
||||||
(0x52 << 1) + 1, /* read */ |
|
||||||
6, |
|
||||||
wmp_sample_data, |
|
||||||
&wmp_result, |
|
||||||
NULL |
|
||||||
}; |
|
||||||
|
|
||||||
|
|
||||||
bool wmp_init(void) |
|
||||||
{ |
|
||||||
if (!i2c_start_transaction(&wmp_init_transaction)) |
|
||||||
return FALSE; |
|
||||||
while (i2c_busy()) ; |
|
||||||
return (wmp_result == I2C_SUCCESS); |
|
||||||
} |
|
||||||
|
|
||||||
unsigned char wmp_calibration_data[0x20]; |
|
||||||
|
|
||||||
bool wmp_read_calibration_data(void) |
|
||||||
{ |
|
||||||
if (!i2c_start_transaction(&wmp_read_cal_transaction)) |
|
||||||
return FALSE; |
|
||||||
while (i2c_busy()); |
|
||||||
return (wmp_result == I2C_SUCCESS); |
|
||||||
} |
|
||||||
|
|
||||||
unsigned int wmp_yaw; |
|
||||||
unsigned int wmp_pitch; |
|
||||||
unsigned int wmp_roll; |
|
||||||
|
|
||||||
unsigned int wmp_yaw_zero; |
|
||||||
unsigned int wmp_pitch_zero; |
|
||||||
unsigned int wmp_roll_zero; |
|
||||||
|
|
||||||
bool wmp_yaw_fast; |
|
||||||
bool wmp_pitch_fast; |
|
||||||
bool wmp_roll_fast; |
|
||||||
|
|
||||||
int accel_x; |
|
||||||
int accel_y; |
|
||||||
int accel_z; |
|
||||||
|
|
||||||
bool wmp_update; |
|
||||||
bool wmp_zero; |
|
||||||
unsigned int wmp_discard; |
|
||||||
|
|
||||||
#define TWO_PI 6.28318531f |
|
||||||
#define DEG_TO_RAD (TWO_PI/360.0f) |
|
||||||
|
|
||||||
/* There's considerable debate about these values, and they may vary
|
|
||||||
* between different models of the Wii Motion Plus. It would be nice |
|
||||||
* to be able to use the calibration data stored on the device itself |
|
||||||
* but we don't know the format yet. |
|
||||||
*/ |
|
||||||
#define SLOW_YAW_STEP (20 / DEG_TO_RAD) |
|
||||||
#define SLOW_PITCH_STEP (20 / DEG_TO_RAD) |
|
||||||
#define SLOW_ROLL_STEP (20 / DEG_TO_RAD) |
|
||||||
#define FAST_YAW_STEP (4 / DEG_TO_RAD) |
|
||||||
#define FAST_PITCH_STEP (4 / DEG_TO_RAD) |
|
||||||
#define FAST_ROLL_STEP (4 / DEG_TO_RAD) |
|
||||||
|
|
||||||
/* The gyro has to stay within this limit in each axis in order to arm */ |
|
||||||
#define GYRO_RATE_THRESHOLD (0.01 / DEG_TO_RAD) |
|
||||||
|
|
||||||
bool wmp_sample(void) |
|
||||||
{ |
|
||||||
if (!i2c_start_transaction(&wmp_sample_transaction)) |
|
||||||
return FALSE; |
|
||||||
|
|
||||||
while (i2c_busy()); |
|
||||||
|
|
||||||
if (wmp_result != I2C_SUCCESS) |
|
||||||
return FALSE; |
|
||||||
|
|
||||||
wmp_result = I2C_IN_PROGRESS; |
|
||||||
|
|
||||||
wmp_yaw = ((wmp_sample_data[3]>>2)<<8) + wmp_sample_data[0]; |
|
||||||
wmp_pitch = ((wmp_sample_data[4]>>2)<<8) + wmp_sample_data[1]; |
|
||||||
wmp_roll = ((wmp_sample_data[5]>>2)<<8) + wmp_sample_data[2]; |
|
||||||
|
|
||||||
/* XXX We don't take into account the fast/slow mode flag here */ |
|
||||||
wmp_yaw_fast = !(wmp_sample_data[3] & 0x2); |
|
||||||
wmp_pitch_fast = !(wmp_sample_data[3] & 0x1); |
|
||||||
wmp_roll_fast = !(wmp_sample_data[4] & 0x2); |
|
||||||
|
|
||||||
return TRUE; |
|
||||||
} |
|
||||||
|
|
||||||
bool wmp_start_sample(void) |
|
||||||
{ |
|
||||||
return i2c_start_transaction(&wmp_sample_transaction); |
|
||||||
} |
|
||||||
|
|
||||||
void wmp_process_gyro_sample(void) |
|
||||||
{ |
|
||||||
float yaw, pitch, roll; |
|
||||||
|
|
||||||
wmp_yaw = ((wmp_sample_data[3]>>2)<<8) + wmp_sample_data[0]; |
|
||||||
wmp_pitch = ((wmp_sample_data[4]>>2)<<8) + wmp_sample_data[1]; |
|
||||||
wmp_roll = ((wmp_sample_data[5]>>2)<<8) + wmp_sample_data[2]; |
|
||||||
|
|
||||||
/* XXX We don't take into account the fast/slow mode flag here */ |
|
||||||
wmp_yaw_fast = !(wmp_sample_data[3] & 0x2); |
|
||||||
wmp_pitch_fast = !(wmp_sample_data[3] & 0x1); |
|
||||||
wmp_roll_fast = !(wmp_sample_data[4] & 0x2); |
|
||||||
|
|
||||||
if (wmp_update) { |
|
||||||
int tmp_yaw = wmp_yaw; |
|
||||||
int tmp_pitch = wmp_pitch; |
|
||||||
int tmp_roll = wmp_roll; |
|
||||||
|
|
||||||
tmp_yaw -= wmp_yaw_zero; |
|
||||||
tmp_pitch -= wmp_pitch_zero; |
|
||||||
tmp_roll -= wmp_roll_zero; |
|
||||||
|
|
||||||
if (wmp_yaw_fast) |
|
||||||
yaw = ((float)tmp_yaw) / FAST_YAW_STEP; |
|
||||||
else |
|
||||||
yaw = ((float)tmp_yaw) / SLOW_YAW_STEP; |
|
||||||
|
|
||||||
if (wmp_pitch_fast) |
|
||||||
pitch = ((float)tmp_pitch) / FAST_PITCH_STEP; |
|
||||||
else |
|
||||||
pitch = ((float)tmp_pitch) / SLOW_PITCH_STEP; |
|
||||||
|
|
||||||
if (wmp_roll_fast) |
|
||||||
roll = ((float)tmp_roll) / FAST_ROLL_STEP; |
|
||||||
else |
|
||||||
roll = ((float)tmp_roll) / SLOW_ROLL_STEP; |
|
||||||
|
|
||||||
dcm_update(roll, pitch, yaw); |
|
||||||
|
|
||||||
if (!status_armed()) { |
|
||||||
if ( (abs(roll) < GYRO_RATE_THRESHOLD) && |
|
||||||
(abs(pitch) < GYRO_RATE_THRESHOLD) && |
|
||||||
(abs(yaw) < GYRO_RATE_THRESHOLD) ) { |
|
||||||
status_set_ready(STATUS_MODULE_GYRO_RATE, TRUE); |
|
||||||
} else { |
|
||||||
status_set_ready(STATUS_MODULE_GYRO_RATE, FALSE); |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
wmp_generation++; |
|
||||||
|
|
||||||
#if SEND_DCM |
|
||||||
if ((wmp_generation % 20) == 0) |
|
||||||
dcm_send_packet(); |
|
||||||
#endif |
|
||||||
|
|
||||||
} else if (wmp_zero) { |
|
||||||
if (wmp_discard) { |
|
||||||
wmp_discard--; |
|
||||||
} else { |
|
||||||
wmp_yaw_zero += wmp_yaw; |
|
||||||
wmp_pitch_zero += wmp_pitch; |
|
||||||
wmp_roll_zero += wmp_roll; |
|
||||||
wmp_generation++; |
|
||||||
if (wmp_generation >= WMP_ZERO_COUNT) { |
|
||||||
wmp_zero = FALSE; |
|
||||||
wmp_update = TRUE; |
|
||||||
wmp_generation = 0; |
|
||||||
wmp_yaw_zero /= WMP_ZERO_COUNT; |
|
||||||
wmp_pitch_zero /= WMP_ZERO_COUNT; |
|
||||||
wmp_roll_zero /= WMP_ZERO_COUNT; |
|
||||||
putstr("Zero finished\r\n"); |
|
||||||
status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE); |
|
||||||
} |
|
||||||
} |
|
||||||
} |
|
||||||
watchdog_kick(WATCHDOG_GYRO); |
|
||||||
} |
|
||||||
|
|
||||||
void wmp_process_accel_sample(void) |
|
||||||
{ |
|
||||||
float x, y, z; |
|
||||||
#if 0 |
|
||||||
float invmag; |
|
||||||
#endif |
|
||||||
|
|
||||||
accel_x = (wmp_sample_data[2]<<2) + ((wmp_sample_data[5]>>3) & 0x02); |
|
||||||
accel_y = (wmp_sample_data[3]<<2) + ((wmp_sample_data[5]>>4) & 0x02); |
|
||||||
accel_z = ((wmp_sample_data[4]<<2) & 0x3f8) + |
|
||||||
((wmp_sample_data[5]>>5) & 0x06); |
|
||||||
|
|
||||||
x = (accel_x - ACCEL_ZERO_X) * ACCEL_SCALE; |
|
||||||
y = (accel_y - ACCEL_ZERO_Y) * ACCEL_SCALE; |
|
||||||
z = (accel_z - ACCEL_ZERO_Z) * ACCEL_SCALE; |
|
||||||
|
|
||||||
#if 0 |
|
||||||
invmag = fisqrt(x*x + y*y + z*z); |
|
||||||
|
|
||||||
x = x * invmag; |
|
||||||
y = y * invmag; |
|
||||||
z = z * invmag; |
|
||||||
#endif |
|
||||||
|
|
||||||
#if 0 |
|
||||||
accel_x = (x * 512.0 + 1000.0); |
|
||||||
accel_y = (y * 512.0 + 1000.0); |
|
||||||
accel_z = (z * 512.0 + 1000.0); |
|
||||||
#endif |
|
||||||
|
|
||||||
#if 0 |
|
||||||
putstr("("); |
|
||||||
putint(accel_x); |
|
||||||
putstr(", "); |
|
||||||
putint(accel_y); |
|
||||||
putstr(", "); |
|
||||||
putint(accel_z); |
|
||||||
putstr(")\r\n"); |
|
||||||
#endif |
|
||||||
|
|
||||||
/* The minus signs are needed because something is upside down.
|
|
||||||
* It might actually be the WMP, but we're defining coordinates based |
|
||||||
* on that so we'll just fudge it here. |
|
||||||
*/ |
|
||||||
dcm_drift_correction(x, -y, -z); |
|
||||||
watchdog_kick(WATCHDOG_ACCEL); |
|
||||||
stick_input(); |
|
||||||
} |
|
||||||
|
|
||||||
void wmp_event_handler(void) |
|
||||||
{ |
|
||||||
if (wmp_result != I2C_SUCCESS) |
|
||||||
return; |
|
||||||
|
|
||||||
wmp_result = I2C_IN_PROGRESS; |
|
||||||
|
|
||||||
if (wmp_sample_data[5] & 0x02) |
|
||||||
wmp_process_gyro_sample(); |
|
||||||
else |
|
||||||
wmp_process_accel_sample(); |
|
||||||
} |
|
||||||
|
|
||||||
void wmp_start_zero(void) |
|
||||||
{ |
|
||||||
wmp_zero = TRUE; |
|
||||||
wmp_update = FALSE; |
|
||||||
wmp_discard = 100; |
|
||||||
wmp_generation = 0; |
|
||||||
putstr("Starting zero\r\n"); |
|
||||||
} |
|
||||||
Loading…
Reference in new issue