Browse Source

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

to 200Hz, add SD card logging, and a number of other changes.
master
Gavan Fantom 11 years ago
parent
commit
e8adff93c1
  1. 9
      Makefile
  2. 21
      crt0.s
  3. 22
      dcm.c
  4. 19
      event.c
  5. 7
      event.h
  6. 12
      fisqrt.c
  7. 10
      i2c.c
  8. 1
      i2c.h
  9. 1
      interrupt.h
  10. 110
      log.c
  11. 324
      main.c
  12. 8
      motor.c
  13. 263
      mpu6050.c
  14. 36
      panic.c
  15. 8
      panic.h
  16. 678
      sdcard.c
  17. 233
      sensors.c
  18. 116
      spi.c
  19. 22
      stick.c
  20. 6
      timer.c
  21. 13
      timer.h
  22. 62
      uart.c
  23. 1
      uart.h
  24. 2
      watchdog.c
  25. 320
      wmp.c

9
Makefile

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

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

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

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

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

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

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

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

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

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

324
main.c

@ -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,84 +241,37 @@ 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());
case 'D':
sensors_dump();
break;
case 'N':
putstr("The time is ");
puthex(timer_read());
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();
init_sdcard();
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");
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("FAIL");
reply("SD card read failed");
}
break;
case 'N':
putstr("The time is ");
puthex(timer_read());
case 'A':
sd_address = read_number();
putstr("SD read address set to 0x");
puthex(sd_address);
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();
break;
case 'W':
for (i = 0; i < 4; i++) {
putstr("Width ");
@ -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

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

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

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

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

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