The unified diff between revisions [d0420ebd..] and [9142f333..] 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 [d0420ebd87c820e33a32b29727989516e15980a8] # new_revision [9142f3330490a5aa00c1686475633b620c2ef5e7] # # patch "main.c" # from [958bdc83e26a4a7dda991daabf259c44079bc7c5] # to [d72da4d2ad3be9c5db9d6019164152e4eea61e43] # ============================================================ --- main.c 958bdc83e26a4a7dda991daabf259c44079bc7c5 +++ main.c d72da4d2ad3be9c5db9d6019164152e4eea61e43 @@ -1,13 +1,19 @@ /* main.c */ -#include "wmp.h" +#include "sensors.h" #include "i2c.h" #include "timer.h" #include "uart.h" #include "interrupt.h" #include "event.h" -#include "stick.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 int *) 0xE002C000)) #define PINSEL1 (*((volatile unsigned int *) 0xE002C004)) @@ -16,17 +22,24 @@ #define SCS (*((volatile unsigned int *) 0xe01fc1a0)) +#define BUTTON_PRESSED (!((FP0XVAL) & 0x00010000)) +char buffer[512]; + void init_pins(void) { - PINSEL0 = 0x00a88055; /* 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.10 and P0.11 assigned to CAP1.[01] */ - PINSEL1 = 0x20000828; /* P0.21 and P0.30 assigned to MAT3.[03] */ - /* P0.17 and P0.18 assigned to CAP1.[23] */ + /* 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; } @@ -40,193 +53,184 @@ void reply(char *str) #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; - } - 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"); + 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"); } -void timer_event_handler(void) +#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) { - wmp_start_sample(); + 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 -void menu_handler(void); - int main(void) { - armed = FALSE; + int i; init_interrupt(); init_uart(); init_i2c(); init_pins(); init_timer(); + init_status(); event_register(EVENT_UART_INPUT, menu_handler); - event_register(EVENT_I2C_COMPLETE, wmp_event_handler); + event_register(EVENT_TIMER, timer_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) + calibrate_escs(); + putstr("prompt> "); - led_set(FALSE); timer_delay_ms(1000); - led_set(TRUE); - timer_delay_ms(1000); - led_set(FALSE); - 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_set_pattern(led_pattern_active); + led_init(); + init_watchdog(); + /* Good luck! */ while (1) { + CHECKPOINT(0); led_update(); - event_dispatch(); + 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 1 +#if 0 continue; /* Yes, let's just ignore UART input now. */ #endif if (c == 0x0a) @@ -237,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()); + 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"); - 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"); + 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 "); @@ -329,36 +286,65 @@ 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: - timer_set_pwm_value(0, 0); - timer_set_pwm_value(1, 0); - timer_set_pwm_value(2, 0); - timer_set_pwm_value(3, 0); + set_thrust(0, 0.0); + set_thrust(1, 0.0); + set_thrust(2, 0.0); + set_thrust(3, 0.0); + power = 0; break; #if 0 case '1' & 0xdf: - timer_set_pwm_value(0, PWM_MAX/2); + 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 '2' & 0xdf: - timer_set_pwm_value(1, PWM_MAX/2); + power++; + power = power % 16; + putstr("Power setting: "); + putint(power); + putstr("\r\n"); + set_thrust(0, ((float)power)/16.0); break; +#endif +#if 0 + case '1' & 0xdf: + set_thrust(0, 0.5); + break; + case '2' & 0xdf: + set_thrust(1, 0.5); + break; case '3' & 0xdf: - timer_set_pwm_value(2, PWM_MAX/2); + set_thrust(2, 0.5); break; case '4' & 0xdf: - timer_set_pwm_value(3, PWM_MAX/2); + set_thrust(3, 0.5); break; case '5' & 0xdf: - timer_set_pwm_value(0, PWM_MAX); + set_thrust(0, 1.0); break; case '6' & 0xdf: - timer_set_pwm_value(1, PWM_MAX); + set_thrust(1, 1.0); break; case '7' & 0xdf: - timer_set_pwm_value(2, PWM_MAX); + set_thrust(2, 1.0); break; case '8' & 0xdf: - timer_set_pwm_value(3, PWM_MAX); + set_thrust(3, 1.0); break; #endif case '9' & 0xdf: