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