The unified diff between revisions [a39fe798..] and [9ca449dd..] is displayed below. It can also be downloaded as a raw diff.
This diff has been restricted to the following files: 'main.c'
# # old_revision [a39fe7980c8f14b70401f4c97f3e10232dce016a] # new_revision [9ca449dd7941ad52e33bdcb5c28b2ba35d54219a] # # patch "main.c" # from [38594d91649f88377c87a52973d831d9ffeafb70] # to [d72da4d2ad3be9c5db9d6019164152e4eea61e43] # ============================================================ --- main.c 38594d91649f88377c87a52973d831d9ffeafb70 +++ main.c d72da4d2ad3be9c5db9d6019164152e4eea61e43 @@ -1,193 +1,238 @@ +/* main.c */ -#include "wmp.h" +#include "sensors.h" #include "i2c.h" #include "timer.h" #include "uart.h" #include "interrupt.h" #include "event.h" +#include "led.h" +#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 char *) 0xE002C000)) +#define PINSEL0 (*((volatile unsigned int *) 0xE002C000)) +#define PINSEL1 (*((volatile unsigned int *) 0xE002C004)) #define FP0XDIR (*((volatile unsigned int *) 0x3FFFC000)) #define FP0XVAL (*((volatile unsigned int *) 0x3FFFC014)) #define SCS (*((volatile unsigned int *) 0xe01fc1a0)) +#define BUTTON_PRESSED (!((FP0XVAL) & 0x00010000)) +char buffer[512]; + void init_pins(void) { - PINSEL0 = 0x00000055; /* P0.0 and P0.1 assigned to UART */ + PINSEL0 = 0x2a09a255; /* P0.0 and P0.1 assigned to UART */ /* P0.2 and P0.3 assigned to I2C */ + /* P0.4 and P0.6 assigned to CAP0.[12] */ + /* P0.7 and P0.9 assigned to MAT2.[02] */ + /* P0.8 assigned to UART1 */ + /* P0.12 and P0.13 assigned to MAT1.[01] */ + /* P0.14 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; } +#ifdef USE_UART void reply(char *str) { putstr(str); putstr("\r\n"); } +#else +#define reply(x) ((void)0) +#endif -unsigned int count = 0; - -void minmax_sample(void) +void timer_event_handler(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; + unsigned int timestamp = timer_read(); - putstr("Sampling min/max values\r\n"); - if (!wmp_sample()) { - putstr("\r\nRead error\r\n"); - return; - } + log_put_header(timestamp); + sensors_start_sample(); +} - fast_roll_min = fast_roll_max = wmp_roll; - fast_pitch_min = fast_pitch_max = wmp_pitch; - fast_yaw_min = fast_yaw_max = wmp_yaw; +void menu_handler(void); - slow_roll_min = slow_roll_max = wmp_roll; - slow_pitch_min = slow_pitch_max = wmp_pitch; - slow_yaw_min = slow_yaw_max = wmp_yaw; +void wait_for_button_pressed(bool target) +{ + bool pressed; - count = 0; + led_set(!target); - 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); - } + /* Very crude debouncing */ + timer_delay_ms(100); + + target = target ? TRUE:FALSE; + + do { + pressed = BUTTON_PRESSED; + } while (pressed != target); + led_set(pressed); } -void average_sample(void) +void calibrate_escs() { - int i; - int roll_total; - int pitch_total; - int yaw_total; + wait_for_button_pressed(0); - putstr("Sampling average values\r\n"); + putstr("Calibration mode\r\n"); - roll_total = 0; - pitch_total = 0; - yaw_total = 0; + wait_for_button_pressed(1); + wait_for_button_pressed(0); - for (i = 0; i < 0x1000; i++) { - if (!wmp_sample()) { - putstr("\r\nRead error\r\n"); - return; + set_thrust(0, 1.0); + set_thrust(1, 1.0); + set_thrust(2, 1.0); + set_thrust(3, 1.0); + putstr("Max throttle set\r\n"); + + wait_for_button_pressed(1); + wait_for_button_pressed(0); + + set_thrust(0, 0.0); + set_thrust(1, 0.0); + set_thrust(2, 0.0); + set_thrust(3, 0.0); + putstr("Zero throttle set\r\n"); + + wait_for_button_pressed(1); + wait_for_button_pressed(0); + + 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(":"); } - roll_total += wmp_roll; - pitch_total += wmp_pitch; - yaw_total += wmp_yaw; - timer_delay_ms(2); + putstr(" "); + puthex(buffer[i]); + if ((i % 16) == 15) { + putstr("\r\n"); + } } - putstr("("); - puthex(roll_total); - putstr(", "); - puthex(pitch_total); - putstr(", "); - puthex(yaw_total); - putstr(")\r\n"); + if ((i % 16) != 0) + putstr("\r\n"); } +#else +#define dump_buffer(a, b, c) ((void)0) +#endif -void menu_handler(void); +int main(void) { + int i; -int main(void) { init_interrupt(); init_uart(); init_i2c(); init_pins(); init_timer(); + init_status(); event_register(EVENT_UART_INPUT, menu_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) + calibrate_escs(); + putstr("prompt> "); + timer_delay_ms(1000); + if (!sensors_init()) + putstr("Sensor initialisation failed\r\n"); + + /* Flight is potentially live after this. */ + timer_set_period(TIMER_MS(5)); +#if 1 + sensors_start_zero(); +#endif + + led_init(); + + init_watchdog(); + + /* Good luck! */ while (1) { - FP0XVAL ^= 0x04000000; - event_dispatch(); + CHECKPOINT(0); + led_update(); + CHECKPOINT(1); + if (!event_dispatch()) + sdcard_poll(); + CHECKPOINT(2); + watchdog_check(); } return 0; } +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) { int i; char c; while (getch(&c)) { +#if 0 + continue; /* Yes, let's just ignore UART input now. */ +#endif if (c == 0x0a) continue; putch(c); @@ -196,85 +241,118 @@ 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()); + case 'D': + sensors_dump(); + break; + case 'N': + putstr("The time is "); + puthex(timer_read()); reply("."); - putstr("I2C register is: "); - puthex(i2c_conreg()); - reply("."); break; case 'I': - putstr("Initialising WMP... "); - if (wmp_init()) - reply("done"); - else - reply("FAIL"); + init_sdcard(); 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"); + 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++) { + putstr("Width "); + putint(i); + putstr(": "); + putint(timer_input(i)); + if (!timer_valid(i)) + putstr(" (invalid)"); + putstr("\r\n"); + } + if (!timer_allvalid()) { + putstr("ALL INVALID!\r\n"); + } + break; +#if 0 + case 'T': + sdcard_start_write(); + break; +#endif case 'L': - minmax_sample(); + spi_drain(); break; - case 'V': - average_sample(); + case '0' & 0xdf: + set_thrust(0, 0.0); + set_thrust(1, 0.0); + set_thrust(2, 0.0); + set_thrust(3, 0.0); + power = 0; 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"); - } +#if 0 + case '1' & 0xdf: + power--; + if (power < 0) + power = 15; + power = power % 16; + putstr("Power setting: "); + putint(power); + putstr("\r\n"); + set_thrust(0, ((float)power)/16.0); break; - case 'N': - putstr("The time is "); - puthex(timer_read()); - reply("."); + case '2' & 0xdf: + power++; + power = power % 16; + putstr("Power setting: "); + putint(power); + putstr("\r\n"); + set_thrust(0, ((float)power)/16.0); break; - case 'P': - putstr("Initialising timer... "); - timer_set_period(10000*TIMER_MS); - reply("done"); +#endif +#if 0 + case '1' & 0xdf: + set_thrust(0, 0.5); break; - case 'E': - event_dispatch(); - reply("done"); + case '2' & 0xdf: + set_thrust(1, 0.5); break; + case '3' & 0xdf: + set_thrust(2, 0.5); + break; + case '4' & 0xdf: + set_thrust(3, 0.5); + break; + case '5' & 0xdf: + set_thrust(0, 1.0); + break; + case '6' & 0xdf: + set_thrust(1, 1.0); + break; + case '7' & 0xdf: + set_thrust(2, 1.0); + break; + case '8' & 0xdf: + set_thrust(3, 1.0); + break; +#endif + case '9' & 0xdf: + timer_set_pwm_invalid(0); + timer_set_pwm_invalid(1); + timer_set_pwm_invalid(2); + timer_set_pwm_invalid(3); + break; default: reply("Unrecognised command."); break;