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