coolfactor/quadrotor
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Bye bye Wii Motion Plus, hello MPU6050. Also, increase control loop

Browse Source 
to 200Hz, add SD card logging, and a number of other changes.
This commit is contained in:
Gavan Fantom
2014-05-06 21:03:48 +00:00
parent b482b85ffd
commit e8adff93c1
25 changed files with 1725 additions and 581 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
Makefile
View File

@@ -3,14 +3,17 @@
NAME=quad
SSRCS=crt0.s
CSRCS=main.c i2c.c wmp.c timer.c interrupt.c uart.c event.c matrix.c dcm.c
CSRCS=main.c i2c.c mpu6050.c timer.c interrupt.c uart.c event.c matrix.c dcm.c
CSRCS+=fisqrt.c stick.c trig.c motor.c led.c watchdog.c panic.c status.c
CSRCS+=thrust.c
CSRCS+=thrust.c sensors.c spi.c sdcard.c log.c
#PROJOPTS=-DUSE_UART -DSEND_DCM -DSTICK_DEBUG_CALIBRATE
PROJOPTS=-DTIMER_CPPM
#PROJOPTS=-DTIMER_CPPM -DUSE_UART -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DSEND_DCM
#PROJOPTS=-DTIMER_CPPM -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DSEND_DCM
#PROJOPTS=-DTIMER_CPPM -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DUSE_UART -DSEND_DCM
COPTIM?=-O1
COPTIM?=-Os
CFLAGS=-march=armv4t -msoft-float $(COPTIM) -Wall -Werror -Wextra $(PROJOPTS)
LDSCRIPT=lpc2103_flash.ld

21
crt0.s
View File

@@ -7,7 +7,7 @@
.equ PLL_P, 2
.equ FLASHCLOCKS, 3 /* 40-60MHz clock */
.equ APB_DIVIDER, 4 /* 1, 2 or 4 */
.equ APB_DIVIDER, 1 /* 1, 2 or 4 */
.equ UND_STACK_SIZE, 0x0004
.equ SVC_STACK_SIZE, 0x0010
@@ -62,6 +62,8 @@
.equ APBDIV_BASE, 0xE01FC100
.equ APBDIV, 0
.equ FP0XVAL, 0x3FFFC014
# True is -1 so we subtract values together.
.equ PLL_LOG_P, (0-(PLL_P>1)-(PLL_P>2)-(PLL_P>4))
.equ PLLCFG_VAL, (PLL_M-1) | (PLL_LOG_P << 5)
@@ -185,10 +187,27 @@ lzi:
b main
# Undefined handlers can just spin for now
# Turn on LED
# ldr r2, =FP0XVAL
# ldr r0, [r2, #0]
# bic r0, r0, #0x04000000
# str r0, [r2, #0]
# b __back
# Turn off LED
# ldr r2, =FP0XVAL
# ldr r0, [r2, #0]
# orr r0, r0, #0x04000000
# str r0, [r2, #0]
# b __back
undefined_handler:
prefetch_abort_handler:
data_abort_handler:
fiq_handler:
mov r0, r14
bl panic
__back:
b __back

22
dcm.c
View File

@@ -11,8 +11,13 @@
#include "motor.h"
#include "status.h"
#include "abs.h"
#include "log.h"
#if 0
#define GRAVITY 9.80665f
#endif
#define GRAVITY 1.0f
#define KP_ROLLPITCH 0.05967
#define KI_ROLLPITCH 0.00001278
@@ -22,6 +27,9 @@
/* Maximum allowed error for arming */
#define ERROR_THRESHOLD 0.20f
#define LOG_MAGIC_DCM_UPDATE 0x00DC111A
#define LOG_MAGIC_DCM_DRIFT 0x00DC111B
/* Implementation of the DCM IMU concept as described by Premerlani
* and Bizard
@@ -36,7 +44,15 @@ float omega_i[3] = {0.0, 0.0, 0.0};
float omega_x, omega_y, omega_z;
float delta_t = 0.01;
float delta_t = 0.005;
void dcm_log(unsigned int magic)
{
int i;
log_put_uint(magic);
for (i = 0; i < 9; i++)
log_put_float(dcm[i]);
}
void dcm_update(float x, float y, float z)
{
@@ -57,6 +73,8 @@ void dcm_update(float x, float y, float z)
matrix_add(dcm, dcm, temp_matrix, 3, 3);
dcm_normalise();
/* dcm_log(LOG_MAGIC_DCM_UPDATE); */
}
void dcm_setvector(float x, float y, float z)
@@ -200,6 +218,8 @@ void dcm_drift_correction(float x, float y, float z)
omega_i[i] += error[i] * (KI_ROLLPITCH * weight);
}
dcm_log(LOG_MAGIC_DCM_DRIFT);
#if 0
putstr("w: ");
putint_s((int)(weight * 100000.0f));

19
event.c
View File

@@ -1,6 +1,7 @@
#include "event.h"
#include "interrupt.h"
#include "types.h"
#include "log.h"
event_handler *event_handler_table[EVENT_MAX+1];
@@ -13,14 +14,18 @@ unsigned int event_pending[EVENT_WORDS];
#define EVENT_BIT(x) (x%EVENT_WORDLEN)
#define EVENT_MASK(x) (1<<EVENT_BIT(x))
/*
* This function must be called with interrupts disabled.
* This will normally be the case as it is typically called from within
* an interrupt handler anyway.
*/
void event_set(unsigned int event)
{
if (event > EVENT_MAX)
return;
interrupt_block();
event_pending[EVENT_WORD(event)] |= EVENT_MASK(event);
interrupt_unblock();
}
static int bitset(unsigned int x)
@@ -56,15 +61,21 @@ void event_clear(unsigned int event)
interrupt_unblock();
}
void event_dispatch(void)
bool event_dispatch(void)
{
unsigned int event;
if (event_get(&event)) {
event_clear(event);
if (event_handler_table[event] != NULL)
if (event_handler_table[event] != NULL) {
log_mark_busy();
(event_handler_table[event])();
log_mark_idle();
}
return TRUE;
}
return FALSE;
}
void event_register(unsigned int event, event_handler *handler)

7
event.h
View File

@@ -4,17 +4,18 @@
#include "types.h"
#define EVENT_TIMER 0
#define EVENT_I2C_COMPLETE 1
#define EVENT_MPU6050_I2C_COMPLETE 1
#define EVENT_UART_INPUT 2
#define EVENT_SDCARD 3
#define EVENT_MAX 2
#define EVENT_MAX 3
typedef void event_handler(void);
void event_set(unsigned int event);
bool event_get(unsigned int *event);
void event_clear(unsigned int event);
void event_dispatch(void);
bool event_dispatch(void);
void event_register(unsigned int event, event_handler *handler);
#endif /* __EVENT_H */

12
fisqrt.c
View File

@@ -6,12 +6,20 @@ float fisqrt(float n)
{
long i;
float x2, y;
union {
float f;
long l;
} u;
x2 = n * 0.5f;
y = n;
i = *(long *)&y;
/* i = *(long *)&y; */
u.f = y;
i = u.l;
i = 0x5f3759df - (i>>1);
y = *(float *)&i;
/* y = *(float *)&i; */
u.l = i;
y = u.f;
y = y * (1.5f - (x2*y*y));
return y;

10
i2c.c
View File

@@ -33,8 +33,12 @@ void init_i2c(void)
IREG(I2CONSET) = 0x40; /* Enable I2C ready for Master Tx */
/* Set up for just under 400kHz */
#ifdef I2C_FAST
#if 0
IWREG(I2SCLL) = (25 * 100);
IWREG(I2SCLH) = (12 * 100);
#endif
IWREG(I2SCLL) = 25 * 4; /* ~400kHz */
IWREG(I2SCLH) = 12 * 4;
#else
IWREG(I2SCLL) = 73; /* ~100kHz */
IWREG(I2SCLH) = 73;
@@ -106,10 +110,10 @@ void __attribute__((interrupt("IRQ"))) i2c_interrupt_handler(void)
IREG(I2CONSET) = STAFLAG;
IREG(I2CONCLR) = STOFLAG | AAFLAG | SIFLAG;
} else {
event_set(i2c_transaction->event);
i2c_transaction = NULL;
IREG(I2CONSET) = STOFLAG;
IREG(I2CONCLR) = STAFLAG | AAFLAG | SIFLAG;
event_set(EVENT_I2C_COMPLETE);
}
}
break;
@@ -137,10 +141,10 @@ void __attribute__((interrupt("IRQ"))) i2c_interrupt_handler(void)
IREG(I2CONSET) = STAFLAG;
IREG(I2CONCLR) = STOFLAG | AAFLAG | SIFLAG;
} else {
event_set(i2c_transaction->event);
i2c_transaction = NULL;
IREG(I2CONSET) = STOFLAG;
IREG(I2CONCLR) = STAFLAG | AAFLAG | SIFLAG;
event_set(EVENT_I2C_COMPLETE);
}
break;
@@ -150,10 +154,10 @@ void __attribute__((interrupt("IRQ"))) i2c_interrupt_handler(void)
case 0x38: /* arbitration lost during SA+W or data */
case 0x00: /* bus error */
*(i2c_transaction->result) = I2C_FAIL;
event_set(i2c_transaction->event);
i2c_transaction = NULL;
IREG(I2CONSET) = STOFLAG;
IREG(I2CONCLR) = STAFLAG | AAFLAG | SIFLAG;
event_set(EVENT_I2C_COMPLETE);
break;
/* We don't handle slave mode */

1
i2c.h
View File

@@ -14,6 +14,7 @@ struct i2c_transaction {
int bytes; /* number of bytes to read or write */
unsigned char *data; /* pointer to the data */
i2c_result *result; /* pointer to store the result */
unsigned int event; /* Which event to set when complete */
struct i2c_transaction *next; /* NULL or next transaction */
};

1
interrupt.h
View File

@@ -36,6 +36,7 @@
#define I_PRIORITY_UART0 1
#define I_PRIORITY_TIMER3 2
#define I_PRIORITY_TIMER0 3
#define I_PRIORITY_SPI1 4
#define interrupt_clear() do { VICVectAddr = 0xff; } while (0)

110
log.c Normal file
View File

@@ -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;
}

326
main.c
View File

@@ -1,6 +1,6 @@
/* main.c */
#include "wmp.h"
#include "sensors.h"
#include "i2c.h"
#include "timer.h"
#include "uart.h"
@@ -10,6 +10,10 @@
#include "status.h"
#include "watchdog.h"
#include "thrust.h"
#include "panic.h"
#include "sdcard.h"
#include "log.h"
#include "spi.h"
#define PINSEL0 (*((volatile unsigned int *) 0xE002C000))
#define PINSEL1 (*((volatile unsigned int *) 0xE002C004))
@@ -20,6 +24,8 @@
#define BUTTON_PRESSED (!((FP0XVAL) & 0x00010000))
char buffer[512];
void init_pins(void)
{
PINSEL0 = 0x2a09a255; /* P0.0 and P0.1 assigned to UART */
@@ -30,10 +36,10 @@ void init_pins(void)
/* P0.12 and P0.13 assigned to MAT1.[01] */
/* P0.14 assigned to SPI1 */
PINSEL1 = 0x00000540; /* P0.19 to P0.21 assigned to SPI1 */
PINSEL1 = 0x00000140; /* P0.19 and P0.20 assigned to SPI1 */
SCS = 1;
FP0XDIR = 0x04000000; /* P0.26 is an output */
FP0XDIR = 0x04200000; /* P0.26 and P0.21 are outputs */
FP0XVAL = 0x0;
}
@@ -47,140 +53,12 @@ void reply(char *str)
#define reply(x) ((void)0)
#endif
unsigned int count = 0;
void minmax_sample(void)
{
int count;
unsigned int fast_roll_min, fast_roll_max;
unsigned int fast_pitch_min, fast_pitch_max;
unsigned int fast_yaw_min, fast_yaw_max;
unsigned int slow_roll_min, slow_roll_max;
unsigned int slow_pitch_min, slow_pitch_max;
unsigned int slow_yaw_min, slow_yaw_max;
putstr("Sampling min/max values\r\n");
if (!wmp_sample()) {
putstr("\r\nRead error\r\n");
return;
}
fast_roll_min = fast_roll_max = wmp_roll;
fast_pitch_min = fast_pitch_max = wmp_pitch;
fast_yaw_min = fast_yaw_max = wmp_yaw;
slow_roll_min = slow_roll_max = wmp_roll;
slow_pitch_min = slow_pitch_max = wmp_pitch;
slow_yaw_min = slow_yaw_max = wmp_yaw;
count = 0;
while (1) {
if (!wmp_sample()) {
putstr("\r\nRead error\r\n");
return;
}
if (wmp_roll_fast) {
if (wmp_roll < fast_roll_min)
fast_roll_min = wmp_roll;
if (wmp_roll > fast_roll_max)
fast_roll_max = wmp_roll;
} else {
if (wmp_roll < slow_roll_min)
slow_roll_min = wmp_roll;
if (wmp_roll > slow_roll_max)
slow_roll_max = wmp_roll;
}
if (wmp_pitch_fast) {
if (wmp_pitch < fast_pitch_min)
fast_pitch_min = wmp_pitch;
if (wmp_pitch > fast_pitch_max)
fast_pitch_max = wmp_pitch;
} else {
if (wmp_pitch < slow_pitch_min)
slow_pitch_min = wmp_pitch;
if (wmp_pitch > slow_pitch_max)
slow_pitch_max = wmp_pitch;
}
if (wmp_yaw_fast) {
if (wmp_yaw < fast_yaw_min)
fast_yaw_min = wmp_yaw;
if (wmp_yaw > fast_yaw_max)
fast_yaw_max = wmp_yaw;
} else {
if (wmp_yaw < slow_yaw_min)
slow_yaw_min = wmp_yaw;
if (wmp_yaw > slow_yaw_max)
slow_yaw_max = wmp_yaw;
}
count++;
if (count > 1000) {
putstr("(");
puthex(slow_roll_min);
putstr(", ");
puthex(slow_pitch_min);
putstr(", ");
puthex(slow_yaw_min);
putstr(") (");
puthex(slow_roll_max);
putstr(", ");
puthex(slow_pitch_max);
putstr(", ");
puthex(slow_yaw_max);
putstr(") (");
puthex(fast_roll_min);
putstr(", ");
puthex(fast_pitch_min);
putstr(", ");
puthex(fast_yaw_min);
putstr(") (");
puthex(fast_roll_max);
putstr(", ");
puthex(fast_pitch_max);
putstr(", ");
puthex(fast_yaw_max);
putstr(") \r");
count = 0;
}
timer_delay_ms(2);
}
}
void average_sample(void)
{
int i;
int roll_total;
int pitch_total;
int yaw_total;
putstr("Sampling average values\r\n");
roll_total = 0;
pitch_total = 0;
yaw_total = 0;
for (i = 0; i < 0x1000; i++) {
if (!wmp_sample()) {
putstr("\r\nRead error\r\n");
return;
}
roll_total += wmp_roll;
pitch_total += wmp_pitch;
yaw_total += wmp_yaw;
timer_delay_ms(2);
}
putstr("(");
puthex(roll_total);
putstr(", ");
puthex(pitch_total);
putstr(", ");
puthex(yaw_total);
putstr(")\r\n");
}
void timer_event_handler(void)
{
wmp_start_sample();
unsigned int timestamp = timer_read();
log_put_header(timestamp);
sensors_start_sample();
}
void menu_handler(void);
@@ -232,7 +110,33 @@ void calibrate_escs()
putstr("Exit calibration mode\r\n");
}
#ifdef USE_UART
void dump_buffer(char *buffer, unsigned int length, unsigned int addr);
void dump_buffer(char *buffer, unsigned int length, unsigned int addr)
{
unsigned int i;
for (i = 0; i < length; i++) {
if ((i % 16) == 0) {
puthex(addr+i);
putstr(":");
}
putstr(" ");
puthex(buffer[i]);
if ((i % 16) == 15) {
putstr("\r\n");
}
}
if ((i % 16) != 0)
putstr("\r\n");
}
#else
#define dump_buffer(a, b, c) ((void)0)
#endif
int main(void) {
int i;
init_interrupt();
init_uart();
init_i2c();
@@ -242,10 +146,13 @@ int main(void) {
event_register(EVENT_UART_INPUT, menu_handler);
event_register(EVENT_I2C_COMPLETE, wmp_event_handler);
event_register(EVENT_TIMER, timer_event_handler);
for (i = 0; i < 10; i++) {
if (init_sdcard())
break;
}
putstr("Your entire life has been a mathematical error... a mathematical error I'm about to correct!\r\n");
if (BUTTON_PRESSED)
@@ -254,12 +161,14 @@ int main(void) {
putstr("prompt> ");
timer_delay_ms(1000);
if (!wmp_init())
putstr("WMP initialisation failed\r\n");
if (!sensors_init())
putstr("Sensor initialisation failed\r\n");
/* Flight is potentially live after this. */
timer_set_period(5*TIMER_MS);
wmp_start_zero();
timer_set_period(TIMER_MS(5));
#if 1
sensors_start_zero();
#endif
led_init();
@@ -267,8 +176,12 @@ int main(void) {
/* Good luck! */
while (1) {
CHECKPOINT(0);
led_update();
event_dispatch();
CHECKPOINT(1);
if (!event_dispatch())
sdcard_poll();
CHECKPOINT(2);
watchdog_check();
}
@@ -276,6 +189,40 @@ int main(void) {
}
static int power = 0;
static unsigned int sd_address = 0;
unsigned int read_number(void)
{
unsigned int number;
unsigned int base;
int digit;
char c;
number = 0;
base = 10;
while (1) {
if (getch(&c)) {
digit = -1;
if ((c == 0x0a) || (c == 0x0d))
break;
putch(c);
if (c == 'x')
base = 16;
if ((c >= '0') && (c <= '9'))
digit = c - '0';
if ((c >= 'A') && (c <= 'F'))
digit = c - 'A' + 10;
if ((digit >= 0) && (digit < (int)base)) {
number = number * base;
number += digit;
}
}
}
putstr("\r\n");
return number;
}
void menu_handler(void)
{
@@ -283,7 +230,7 @@ void menu_handler(void)
char c;
while (getch(&c)) {
#if 1
#if 0
continue; /* Yes, let's just ignore UART input now. */
#endif
if (c == 0x0a)
@@ -294,83 +241,36 @@ void menu_handler(void)
case 0x0a:
case 0x0d:
break;
case 'A':
reply("apple");
break;
case 'C':
count++;
putstr("The current count is ");
putint(count);
reply(".");
break;
case 'H':
case '?':
reply("Help is not available. Try a psychiatrist.");
break;
case 'T':
putstr(" I2C status is: ");
puthex(i2c_statreg());
reply(".");
putstr("I2C register is: ");
puthex(i2c_conreg());
reply(".");
break;
case 'I':
putstr("Initialising WMP... ");
if (wmp_init())
reply("done");
else
reply("FAIL");
break;
case 'M':
putstr("Reading from WMP... ");
if (wmp_sample()) {
putstr("(");
puthex(wmp_roll);
putstr(", ");
puthex(wmp_pitch);
putstr(", ");
puthex(wmp_yaw);
reply(").");
} else
reply("FAIL");
break;
case 'L':
minmax_sample();
break;
case 'V':
average_sample();
break;
case 'D':
putstr("Reading calibration data... ");
if (wmp_read_calibration_data()) {
putstr("\r\n");
for (i = 0; i < 0x10 ; i++) {
puthex(wmp_calibration_data[i]);
putstr(" ");
}
putstr("\r\n");
for (i = 0x10; i < 0x20 ; i++) {
puthex(wmp_calibration_data[i]);
putstr(" ");
}
putstr("\r\n");
} else {
reply("FAIL");
}
sensors_dump();
break;
case 'N':
putstr("The time is ");
puthex(timer_read());
reply(".");
break;
case 'P':
putstr("Initialising timer... ");
/* We want a 100Hz loop but two samples per iteration.
* So, we go for 200Hz. */
timer_set_period(5*TIMER_MS);
reply("done");
wmp_start_zero();
case 'I':
init_sdcard();
break;
case 'R':
sd_address = 0;
case 'S':
if (sdcard_read(sd_address++, buffer, 512)) {
dump_buffer(buffer, 512, (sd_address-1) * 512);
reply ("SD card read success");
} else {
reply("SD card read failed");
}
break;
case 'A':
sd_address = read_number();
putstr("SD read address set to 0x");
puthex(sd_address);
reply(".");
break;
case 'W':
for (i = 0; i < 4; i++) {
@@ -386,6 +286,14 @@ void menu_handler(void)
putstr("ALL INVALID!\r\n");
}
break;
#if 0
case 'T':
sdcard_start_write();
break;
#endif
case 'L':
spi_drain();
break;
case '0' & 0xdf:
set_thrust(0, 0.0);
set_thrust(1, 0.0);

8
motor.c
View File

@@ -5,6 +5,7 @@
#include "dcm.h"
#include "uart.h"
#include "status.h"
#include "log.h"
float integral[3] = {0.0f, 0.0f, 0.0f};
float last[3];
@@ -33,7 +34,7 @@ void motor_pid_update(float troll, float mroll,
{
float derivative[3];
float out[3];
float motor[3];
float motor[4];
float roll, pitch, yaw;
float error, max_error;
float min_motor;
@@ -145,6 +146,11 @@ void motor_pid_update(float troll, float mroll,
set_thrust(1, motor[1]);
set_thrust(2, motor[2]);
set_thrust(3, motor[3]);
log_put_uint16((unsigned int) (motor[0] * 65535));
log_put_uint16((unsigned int) (motor[1] * 65535));
log_put_uint16((unsigned int) (motor[2] * 65535));
log_put_uint16((unsigned int) (motor[3] * 65535));
}
void motor_kill(void) {

263
mpu6050.c Normal file
View File

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

36
panic.c
View File

@@ -13,17 +13,38 @@
#include "motor.h"
#include "led.h"
#ifdef PANIC_CHECKPOINT
unsigned int checkpoint;
#endif
#ifdef PANIC_32BIT
#define PANIC_BITS 32
#else
#define PANIC_BITS 16
#endif
#ifdef PANIC_32BIT
led_pattern led_pattern_panic[] = {100, 100, 100, 100, 100, 100, 100, 100,
100, 100, 100, 100, 100, 100, 100, 100,
100, 100, 100, 100, 100, 100, 100, 100,
100, 100, 100, 100, 100, 100, 100, 100,
100, 100, 100, 100, 100, 100, 100, 100,
100, 100, 100, 100, 100, 100, 100, 100,
100, 100, 100, 100, 100, 100, 100, 100,
100, 100, 100, 100, 100, 100, 100, 3000, 0};
#else
led_pattern led_pattern_panic[] = {100, 100, 100, 100, 100, 100, 100, 100,
100, 100, 100, 100, 100, 100, 100, 100,
100, 100, 100, 100, 100, 100, 100, 100,
100, 100, 100, 100, 100, 100, 100, 3000, 0};
#endif
/* Take the lower 16 bits and make a pattern of them, MSB first */
static void panic_create_pattern(led_pattern *pattern, unsigned int reason)
{
int i;
for (i = 0; i < 16; i++) {
if (reason & (1<<(15-i))) {
for (i = 0; i < PANIC_BITS; i++) {
if (reason & (1<<((PANIC_BITS-1)-i))) {
pattern[2*i] = 400;
pattern[2*i+1] = 100;
} else {
@@ -32,19 +53,16 @@ static void panic_create_pattern(led_pattern *pattern, unsigned int reason)
}
if ((i % 4) == 3)
pattern[2*i+1] += 500;
if (i == 15)
if (i == (PANIC_BITS-1))
pattern[2*i+1] += 2500;
}
}
void panic(unsigned int reason)
{
/*
* We may one day be able to do something with the reason, like emit
* a final deathbed confession. So we'll provide the reasons in the
* caller and just ignore them for now.
*/
(void)reason;
#if PANIC_CHECKPOINT
reason = checkpoint;
#endif
motor_kill();

8
panic.h
View File

@@ -6,3 +6,11 @@ void panic(unsigned int reason);
/* Panic code goes in the low 8 bits */
#define PANIC_WATCHDOG_TIMEOUT 0x100
#define PANIC_SENSOR_FAIL 0x200
#ifdef PANIC_CHECKPOINT
extern unsigned int checkpoint;
#define CHECKPOINT(x) do { checkpoint = (x); } while (0)
#else
#define CHECKPOINT(x)
#endif

678
sdcard.c Normal file
View File

@@ -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;
}

233
sensors.c Normal file
View File

@@ -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");
}

116
spi.c Normal file
View File

@@ -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");
}

22
stick.c
View File

@@ -12,9 +12,9 @@
#include "timer.h"
#include "trig.h"
#include "motor.h"
#include "wmp.h"
#include "status.h"
#include "watchdog.h"
#include "log.h"
#define TWO_PI 6.28318531f
#define PI 3.14159265f
@@ -105,11 +105,23 @@ void stick_debug_calibrate()
void stick_input(void) {
float x, y, z, throttle;
unsigned int xi, yi, zi, throttlei;
if (timer_allvalid()) {
x = timer_input(0);
y = timer_input(1);
throttle = timer_input(2);
z = timer_input(3);
xi = timer_input(0);
yi = timer_input(1);
throttlei = timer_input(2);
zi = timer_input(3);
log_put_uint16(xi);
log_put_uint16(yi);
log_put_uint16(throttlei);
log_put_uint16(zi);
x = xi;
y = yi;
throttle = throttlei;
z = zi;
#ifdef STICK_DEBUG_CALIBRATE
if ((stick_counter % 100) == 0)

6
timer.c
View File

@@ -105,7 +105,7 @@ void init_timer(void)
T2REG(TCR) = TCR_ENABLE | TCR_RESET;
T2REG(CTCR) = 0; /* Use PCLK */
T2WREG(PR) = 0; // Prescaling
T2WREG(PR) = 3; // Prescaling
T2WREG(PC) = 0; // Reset the prescale counter
T2WREG(TC) = 0; // Reset the counter
@@ -120,7 +120,7 @@ void init_timer(void)
T1REG(TCR) = TCR_ENABLE | TCR_RESET;
T1REG(CTCR) = 0; /* Use PCLK */
T1WREG(PR) = 0; // Prescaling
T1WREG(PR) = 3; // Prescaling
T1WREG(PC) = 0; // Reset the prescale counter
T1WREG(TC) = 0; // Reset the counter
@@ -151,7 +151,7 @@ void timer_delay_clocks(unsigned int clocks)
void timer_set_period(unsigned int period)
{
interrupt_register(TIMER3, timer_interrupt_handler);
T3WREG(MR0) = period;
T3WREG(MR0) = period-1;
T3WREG(MCR) = MR0I | MR0R;
T3WREG(TC) = 0;
}

13
timer.h
View File

@@ -3,13 +3,14 @@
#include "types.h"
#define TIMER_PCLK 14745600
#define TIMER_PRESCALE 9215
#define TIMER0_PRESCALE 0
#define TIMER_PCLK 58982400
#define TIMER_PRESCALE 36863
#define TIMER0_PRESCALE 3
#define TIMER_SECOND (TIMER_PCLK/(TIMER_PRESCALE+1))
#define TIMER_MS (TIMER_SECOND/1000)
#define TIMER_US (TIMER_SECOND/1000000)
/* Since we're using awkward numbers, this gives better accuracy */
#define TIMER_MS(x) ((x) * TIMER_SECOND / 1000)
#define TIMER_US(x) ((x) * TIMER_SECOND / 1000000)
#define TIMER0_SECOND (TIMER_PCLK/(TIMER0_PRESCALE+1))
#define TIMER0_MS (TIMER0_SECOND/1000)
@@ -21,7 +22,7 @@
#endif
#define PWM_PERIOD ((4*PWM_MAX)+1)
#define TIMER_INPUT_TIMEOUT (TIMER_PCLK/10)
#define TIMER_INPUT_TIMEOUT (TIMER0_SECOND/10)
#define TIMER_CPPM_SYNC 40000
#define TIMER_CH(x) (timer_map[x])

62
uart.c
View File

@@ -2,6 +2,8 @@
#include "types.h"
#include "interrupt.h"
#include "event.h"
#include "led.h"
#include "panic.h"
#define UARTBASE 0xE000C000
@@ -30,10 +32,10 @@
char uart_txbuf[UART_TXBUFSIZE];
char uart_rxbuf[UART_RXBUFSIZE];
volatile int uart_txread;
volatile int uart_txwrite;
volatile int uart_rxread;
volatile int uart_rxwrite;
volatile unsigned int uart_txread;
volatile unsigned int uart_txwrite;
volatile unsigned int uart_rxread;
volatile unsigned int uart_rxwrite;
volatile bool tx_running;
void __attribute__((interrupt("IRQ"))) uart_interrupt_handler(void);
@@ -46,7 +48,7 @@ void init_uart(void)
UREG(LCR) = 0x80;
UREG(DLM) = 0x00;
UREG(DLL) = 0x08; /* 14745600 / (16*115200) */
UREG(DLL) = 0x20; /* 58982400 / (16*115200) */
UREG(LCR) = 0x13;
UREG(FCR) = 0x07;
@@ -61,9 +63,42 @@ void init_uart(void)
}
void putch(char c) {
CHECKPOINT(4);
/* Wait for space in the buffer */
while (uart_txread == ((uart_txwrite+1) % UART_TXBUFSIZE));
while (uart_txread == ((uart_txwrite+1) % UART_TXBUFSIZE)) ;
interrupt_block();
if (uart_txread == uart_txwrite) {
if (U0THRE) {
tx_running = TRUE;
UREG(THR) = c;
interrupt_unblock();
CHECKPOINT(5);
return;
}
}
uart_txbuf[uart_txwrite] = c;
uart_txwrite = (uart_txwrite + 1) % UART_TXBUFSIZE;
if (!tx_running) {
if (uart_txread != uart_txwrite) {
tx_running = TRUE;
uart_txread = (uart_txread + 1) % UART_TXBUFSIZE;
UREG(THR) = c;
}
}
interrupt_unblock();
CHECKPOINT(5);
}
void putch_irq(char c) {
/* Hope for space in the buffer */
// if (uart_txread == ((uart_txwrite+1) % UART_TXBUFSIZE))
// return;
#if 1
if (uart_txread == uart_txwrite) {
if (U0THRE) {
tx_running = TRUE;
@@ -82,6 +117,9 @@ void putch(char c) {
UREG(THR) = c;
}
}
#else
UREG(THR) = c;
#endif
}
void __attribute__((interrupt("IRQ"))) uart_interrupt_handler(void)
@@ -93,10 +131,12 @@ void __attribute__((interrupt("IRQ"))) uart_interrupt_handler(void)
* to treat them as such in this handler, so let the compiler
* have an easier time.
*/
int local_txwrite;
int local_txread;
int local_rxwrite;
int local_rxread;
unsigned int local_txwrite;
unsigned int local_txread;
unsigned int local_rxwrite;
unsigned int local_rxread;
CHECKPOINT(((checkpoint & 0x00ff) | 0x0100));
source = UREG(IIR);
@@ -136,6 +176,8 @@ void __attribute__((interrupt("IRQ"))) uart_interrupt_handler(void)
break;
}
CHECKPOINT((checkpoint & 0x00ff) | 0x0200);
interrupt_clear();
}

1
uart.h
View File

@@ -6,6 +6,7 @@
#ifdef USE_UART
void init_uart(void);
void putch(char c);
void putch_irq(char c);
void putstr(char *s);
void putint(unsigned int n);
void putint_s(int n);

2
watchdog.c
View File

@@ -30,7 +30,7 @@ void watchdog_check(void)
int i;
/* XXX not yet */
/* return; */
return;
for (i = 0; i < WATCHDOG_MODULES; i++) {
if ((signed int)(watchdog_last_seen[i] + WATCHDOG_TIMEOUT
- time) < 0) {

320
wmp.c
View File

@@ -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");
}