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.
340 lines
6.1 KiB
340 lines
6.1 KiB
|
|
#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; |
|
int fast_roll_min, fast_roll_max; |
|
int fast_pitch_min, fast_pitch_max; |
|
int fast_yaw_min, fast_yaw_max; |
|
int slow_roll_min, slow_roll_max; |
|
int slow_pitch_min, slow_pitch_max; |
|
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; |
|
}
|
|
|