|
|
|
|
|
|
|
#include "wmp.h"
|
|
|
|
#include "i2c.h"
|
|
|
|
#include "timer.h"
|
|
|
|
|
|
|
|
#define UARTBASE 0xE000C000
|
|
|
|
|
|
|
|
#define RBR 0x00
|
|
|
|
#define THR 0x00
|
|
|
|
#define DLL 0x00
|
|
|
|
#define DLM 0x04
|
|
|
|
#define IER 0x04
|
|
|
|
#define IIR 0x08
|
|
|
|
#define FCR 0x08
|
|
|
|
|
|
|
|
#define LCR 0x0c
|
|
|
|
#define LSR 0x14
|
|
|
|
#define SCR 0x1c
|
|
|
|
#define ACR 0x20
|
|
|
|
#define FDR 0x28
|
|
|
|
#define TER 0x30
|
|
|
|
|
|
|
|
#define UREG(x) (((volatile unsigned char *)UARTBASE)[x])
|
|
|
|
|
|
|
|
#define U0THRE ((UREG(LSR) & (1<<5))) /* UART0 transmitter holding register is empty */
|
|
|
|
#define U0DR ((UREG(LSR) & (1<<0))) /* UART0 data ready */
|
|
|
|
|
|
|
|
#define PINSEL0 (*((volatile unsigned char *) 0xE002C000))
|
|
|
|
|
|
|
|
void init_uart(void)
|
|
|
|
{
|
|
|
|
UREG(FDR) = 0x10; /* DivAddVal = 0, MulVal = 1 */
|
|
|
|
|
|
|
|
UREG(LCR) = 0x80;
|
|
|
|
UREG(DLM) = 0x00;
|
|
|
|
UREG(DLL) = 0x08; /* 14745600 / (16*115200) */
|
|
|
|
UREG(LCR) = 0x13;
|
|
|
|
UREG(FCR) = 0x07;
|
|
|
|
}
|
|
|
|
|
|
|
|
void init_pins(void)
|
|
|
|
{
|
|
|
|
PINSEL0 = 0x00000055; /* P0.0 and P0.1 assigned to UART */
|
|
|
|
/* P0.2 and P0.3 assigned to I2C */
|
|
|
|
}
|
|
|
|
|
|
|
|
void putch(char c) {
|
|
|
|
while (!U0THRE);
|
|
|
|
UREG(THR) = c;
|
|
|
|
}
|
|
|
|
|
|
|
|
void putstr(char *s) {
|
|
|
|
while (*s) putch(*s++);
|
|
|
|
}
|
|
|
|
|
|
|
|
void putint(unsigned int n) {
|
|
|
|
char s[11];
|
|
|
|
int i;
|
|
|
|
|
|
|
|
i = 10;
|
|
|
|
s[i] = '\0';
|
|
|
|
|
|
|
|
do {
|
|
|
|
s[--i] = n % 10 + '0';
|
|
|
|
} while ((n /= 10) > 0);
|
|
|
|
|
|
|
|
putstr(s+i);
|
|
|
|
}
|
|
|
|
|
|
|
|
void puthex(unsigned int n) {
|
|
|
|
char s[9];
|
|
|
|
int i;
|
|
|
|
|
|
|
|
i = 8;
|
|
|
|
s[i] = '\0';
|
|
|
|
|
|
|
|
do {
|
|
|
|
int x = n % 16;
|
|
|
|
if (x > 9)
|
|
|
|
x += 'A' - '0' - 10;
|
|
|
|
s[--i] = x + '0';
|
|
|
|
} while ((n /= 16) > 0);
|
|
|
|
|
|
|
|
putstr(s+i);
|
|
|
|
}
|
|
|
|
|
|
|
|
char getch(void) {
|
|
|
|
while (!U0DR);
|
|
|
|
return UREG(RBR);
|
|
|
|
}
|
|
|
|
|
|
|
|
void reply(char *str)
|
|
|
|
{
|
|
|
|
putstr(str);
|
|
|
|
putstr("\r\n");
|
|
|
|
}
|
|
|
|
|
|
|
|
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");
|
|
|
|
}
|
|
|
|
|
|
|
|
int main(void) {
|
|
|
|
int i;
|
|
|
|
init_uart();
|
|
|
|
init_i2c();
|
|
|
|
init_pins();
|
|
|
|
init_timer();
|
|
|
|
putstr("Your entire life has been a mathematical error... a mathematical error I'm about to correct!\r\n");
|
|
|
|
|
|
|
|
while (1) {
|
|
|
|
char c;
|
|
|
|
putstr("prompt> ");
|
|
|
|
c = getch();
|
|
|
|
if (c == 0x0a)
|
|
|
|
continue;
|
|
|
|
putch(c);
|
|
|
|
putstr("\r\n");
|
|
|
|
switch (c & 0xdf) {
|
|
|
|
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 was: ");
|
|
|
|
puthex(i2cstat);
|
|
|
|
putstr(" I2C status is: ");
|
|
|
|
puthex(i2c_statreg());
|
|
|
|
reply(".");
|
|
|
|
putstr("I2C register is: ");
|
|
|
|
puthex(i2c_conreg());
|
|
|
|
reply(".");
|
|
|
|
break;
|
|
|
|
case 'S':
|
|
|
|
putstr("Sending START... ");
|
|
|
|
if (i2c_send_start())
|
|
|
|
reply("OK");
|
|
|
|
else
|
|
|
|
reply("FAIL");
|
|
|
|
break;
|
|
|
|
case 'O':
|
|
|
|
putstr("Sending STOP... ");
|
|
|
|
i2c_send_stop();
|
|
|
|
reply("sent");
|
|
|
|
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");
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
case 'N':
|
|
|
|
putstr("The time is ");
|
|
|
|
puthex(timer_read());
|
|
|
|
reply(".");
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
reply("Unrecognised command.");
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|