Quadrotor from scratch
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

363 lines
6.4 KiB

/* main.c */
#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 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 = 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 = 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
void timer_event_handler(void)
{
unsigned int timestamp = timer_read();
log_put_header(timestamp);
sensors_start_sample();
}
void menu_handler(void);
void wait_for_button_pressed(bool target)
{
bool pressed;
led_set(!target);
/* Very crude debouncing */
timer_delay_ms(100);
target = target ? TRUE:FALSE;
do {
pressed = BUTTON_PRESSED;
} while (pressed != target);
led_set(pressed);
}
void calibrate_escs()
{
wait_for_button_pressed(0);
putstr("Calibration mode\r\n");
wait_for_button_pressed(1);
wait_for_button_pressed(0);
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(":");
}
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();
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) {
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);
putstr("\r\n");
switch (c & 0xdf) {
case 0x0a:
case 0x0d:
break;
case 'H':
case '?':
reply("Help is not available. Try a psychiatrist.");
break;
case 'D':
sensors_dump();
break;
case 'N':
putstr("The time is ");
puthex(timer_read());
reply(".");
break;
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++) {
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':
spi_drain();
break;
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;
#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 '2' & 0xdf:
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:
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;
}
putstr("prompt> ");
}
}