coolfactor/quadrotor
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

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

Browse Source 
to 200Hz, add SD card logging, and a number of other changes.
This commit is contained in:
Gavan Fantom
2014-05-06 21:03:48 +00:00
parent b482b85ffd
commit e8adff93c1
25 changed files with 1725 additions and 581 deletions
Download Patch File Download Diff File Expand all files Collapse all files

326
main.c
View File

@@ -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,83 +241,36 @@ 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());
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();
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");
} else {
reply("FAIL");
}
sensors_dump();
break;
case 'N':
putstr("The time is ");
puthex(timer_read());
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();
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++) {
@@ -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);