From b3f0bbf7c96125acd3014db45fd13ba0873bcfc4 Mon Sep 17 00:00:00 2001 From: Gavan Fantom Date: Sun, 15 May 2011 22:22:50 +0000 Subject: [PATCH] More hacking: * initial Wii Motion+ support * initial timer support --- Makefile | 2 +- i2c.c | 36 ++++++--- i2c.h | 6 ++ main.c | 220 ++++++++++++++++++++++++++++++++++++++++++++++--------- timer.c | 53 ++++++++++++++ timer.h | 18 +++++ types.h | 5 ++ wmp.c | 100 +++++++++++++++++++++++++ wmp.h | 20 +++++ 9 files changed, 413 insertions(+), 47 deletions(-) create mode 100644 timer.c create mode 100644 timer.h create mode 100644 wmp.c create mode 100644 wmp.h diff --git a/Makefile b/Makefile index 60da885..f09cf45 100644 --- a/Makefile +++ b/Makefile @@ -3,7 +3,7 @@ NAME=quad SSRCS=crt0.s -CSRCS=main.c i2c.c +CSRCS=main.c i2c.c wmp.c timer.c CFLAGS=-march=armv4t -msoft-float -O1 diff --git a/i2c.c b/i2c.c index 9620d2d..b03541f 100644 --- a/i2c.c +++ b/i2c.c @@ -29,8 +29,13 @@ void init_i2c(void) IWREG(I2SCLL) = (25 * 100); IWREG(I2SCLH) = (12 * 100); #else +# if 0 IWREG(I2SCLL) = 1475; /* ~5kHz */ IWREG(I2SCLH) = 1475; +# else + IWREG(I2SCLL) = 73; /* ~100kHz */ + IWREG(I2SCLH) = 73; +# endif #endif } @@ -45,11 +50,6 @@ int i2c_wait(void) return stat; } -void i2c_go(void) -{ - IREG(I2CONCLR) = SIFLAG; -} - int i2c_conreg(void) { return IREG(I2CONSET); @@ -64,14 +64,12 @@ bool i2c_send_start(void) { IREG(I2CONCLR) = STOFLAG | STAFLAG; IREG(I2CONSET) = STAFLAG; -/* i2c_go(); */ switch (i2c_wait()) { case 0x08: case 0x10: -/* IREG(I2CONCLR) = STAFLAG; */ return TRUE; default: -/* IREG(I2CONCLR) = STAFLAG; */ + i2c_send_stop(); return FALSE; } } @@ -84,12 +82,30 @@ bool i2c_send_address(int addr, bool write) bool i2c_send_data(int data) { IREG(I2DAT) = data; - IREG(I2CONCLR) = STAFLAG | SIFLAG; + IREG(I2CONCLR) = STAFLAG | STOFLAG | SIFLAG; switch (i2c_wait()) { case 0x18: case 0x28: + case 0x40: return TRUE; default: + i2c_send_stop(); + return FALSE; + } +} + +bool i2c_receive_data(int *data, bool last) +{ + if (!last) + IREG(I2CONSET) = AAFLAG; + IREG(I2CONCLR) = STAFLAG | STOFLAG | SIFLAG | (last?AAFLAG:0); + switch (i2c_wait()) { + case 0x50: + case 0x58: + *data = IREG(I2DAT); + return TRUE; + default: + i2c_send_stop(); return FALSE; } } @@ -98,7 +114,7 @@ void i2c_send_stop(void) { IREG(I2CONCLR) = STAFLAG | AAFLAG; IREG(I2CONSET) = STOFLAG; - i2c_go(); + IREG(I2CONCLR) = SIFLAG; /* Don't think we need to wait here. Could be wrong. */ } diff --git a/i2c.h b/i2c.h index 45a7bc4..5bb81aa 100644 --- a/i2c.h +++ b/i2c.h @@ -1,3 +1,6 @@ +#ifndef __I2C_H +#define __I2C_H + #include "types.h" void init_i2c(void); extern int i2cstat; @@ -8,4 +11,7 @@ int i2c_statreg(void); bool i2c_send_start(void); bool i2c_send_address(int addr, bool write); bool i2c_send_data(int data); +bool i2c_receive_data(int *data, bool last); void i2c_send_stop(void); + +#endif /* __I2C_H */ diff --git a/main.c b/main.c index 3384eb4..38e8712 100644 --- a/main.c +++ b/main.c @@ -1,5 +1,7 @@ +#include "wmp.h" #include "i2c.h" +#include "timer.h" #define UARTBASE 0xE000C000 @@ -23,20 +25,6 @@ #define U0THRE ((UREG(LSR) & (1<<5))) /* UART0 transmitter holding register is empty */ #define U0DR ((UREG(LSR) & (1<<0))) /* UART0 data ready */ - -#define I2CBASE 0xE001C000 - -#define I2CONSET 0x00 -#define I2STAT 0x04 -#define I2DAT 0x08 -#define I2ADR 0x0c -#define I2SCLH 0x10 -#define I2SCLL 0x14 -#define I2CONCLR 0x18 - -#define IREG(x) (((volatile unsigned char *)I2CBASE)[x]) - - #define PINSEL0 (*((volatile unsigned char *) 0xE002C000)) void init_uart(void) @@ -109,10 +97,142 @@ void reply(char *str) 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; + int data; 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) { @@ -140,17 +260,15 @@ int main(void) { case '?': reply("Help is not available. Try a psychiatrist."); break; - case 'R': - putstr("I2C register is: "); - puthex(i2c_conreg()); - reply("."); - 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... "); @@ -159,29 +277,59 @@ int main(void) { else reply("FAIL"); break; - case 'D': - putstr("Sending address... "); - if (i2c_send_address(0x53, TRUE)) - reply("OK"); + 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 'B': - putstr("Sending bytes... "); - if (i2c_send_data(0xfe)) - reply("OK"); - else + 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"); - - if (i2c_send_data(0x04)) - reply("OK"); - else + 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 'O': - putstr("Sending STOP... "); - i2c_send_stop(); - reply("sent"); + case 'N': + putstr("The time is "); + puthex(timer_read()); + reply("."); break; default: reply("Unrecognised command."); diff --git a/timer.c b/timer.c new file mode 100644 index 0000000..aab6ff3 --- /dev/null +++ b/timer.c @@ -0,0 +1,53 @@ +#include "timer.h" + +#define TIMER0BASE 0xE0004000 +#define TIMER1BASE 0xE0008000 + +#define IR 0x00 +#define TCR 0x04 +#define TC 0x08 +#define PR 0x0c +#define PC 0x10 +#define MCR 0x14 +#define MR0 0x18 +#define MR1 0x1C +#define MR2 0x20 +#define MR3 0x24 +#define CCR 0x28 +#define CR0 0x2C +#define CR1 0x30 +#define CR2 0x34 +#define CR3 0x38 +#define EMR 0x3C +#define CTCR 0x70 +#define PWM 0x74 + +#define TREG(x) (((volatile unsigned char *)TIMER0BASE)[x]) +#define TWREG(x) (((volatile unsigned int *)TIMER0BASE)[(x)/sizeof(unsigned int)]) + +#define TCR_ENABLE (1<<0) +#define TCR_RESET (1<<1) + +void init_timer(void) +{ + TREG(TCR) = TCR_ENABLE | TCR_RESET; + + TREG(CTCR) = 0; /* Use PCLK */ + TREG(TC) = 0; + TREG(PR) = TIMER_PRESCALE ; + TREG(PC) = 0; + + TREG(TCR) = TCR_ENABLE; +} + +unsigned int timer_read(void) +{ + return TWREG(TC); +} + +void timer_delay_clocks(unsigned int clocks) +{ + signed int time = TWREG(TC) + clocks; + while (((signed int) (time-TWREG(TC))) > 0); +} + diff --git a/timer.h b/timer.h new file mode 100644 index 0000000..0468c30 --- /dev/null +++ b/timer.h @@ -0,0 +1,18 @@ +#ifndef __TIMER_H +#define __TIMER_H + +#define TIMER_PCLK 14745600 +#define TIMER_PRESCALE 0 + +#define TIMER_SECOND (TIMER_PCLK/(TIMER_PRESCALE+1)) +#define TIMER_MS (TIMER_SECOND/1000) +#define TIMER_US (TIMER_SECOND/1000000) + +void init_timer(void); +unsigned int timer_read(void); +void timer_delay_clocks(unsigned int clocks); + +#define timer_delay_us(x) timer_delay_clocks((x)*TIMER_US) +#define timer_delay_ms(x) timer_delay_clocks((x)*TIMER_MS) + +#endif /* __TIMER_H */ diff --git a/types.h b/types.h index 680c9a6..6468b8d 100644 --- a/types.h +++ b/types.h @@ -1,3 +1,8 @@ +#ifndef __TYPES_H +#define __TYPES_H + typedef int bool; #define TRUE 1 #define FALSE 0 + +#endif /* __TYPES_H */ diff --git a/wmp.c b/wmp.c new file mode 100644 index 0000000..2292cf9 --- /dev/null +++ b/wmp.c @@ -0,0 +1,100 @@ + +#include "wmp.h" +#include "i2c.h" + +bool wmp_init(void) +{ + if (!i2c_send_start()) + return FALSE; + if (!i2c_send_address(0x53, TRUE)) + return FALSE; + if (!i2c_send_data(0xfe)) + return FALSE; + if (!i2c_send_data(0x04)) + return FALSE; + i2c_send_stop(); +} + +unsigned char wmp_calibration_data[0x20]; + +bool wmp_read_calibration_data(void) +{ + int i; + + if (!i2c_send_start()) + return FALSE; + if (!i2c_send_address(0x53, TRUE)) + return FALSE; + if (!i2c_send_data(0x20)) + return FALSE; + i2c_send_stop(); + + if (!i2c_send_start()) + return FALSE; + if (!i2c_send_address(0x53, FALSE)) + return FALSE; + for (i = 0; i < 0x20; i++) { + unsigned int data; + if (!i2c_receive_data(&data, (i == 0x1f))) + return FALSE; + wmp_calibration_data[i] = data; + } + i2c_send_stop(); + return TRUE; +} + +unsigned int wmp_yaw; +unsigned int wmp_pitch; +unsigned int wmp_roll; + +bool wmp_yaw_fast; +bool wmp_pitch_fast; +bool wmp_roll_fast; + +/* There's considerable debate about these values, and they may vary + * between different models of the Wii Motion Plus. It would be nice + * to be able to use the calibration data stored on the device itself + * but we don't know the format yet. + */ +#define SLOW_YAW_STEP (1000/20) +#define SLOW_PITCH_STEP (1000/20) +#define SLOW_ROLL_STEP (1000/20) +#define FAST_YAW_STEP (1000/4) +#define FAST_PITCH_STEP (1000/4) +#define FAST_ROLL_STEP (1000/4) + +bool wmp_sample(void) +{ + int i; + unsigned int b[6]; + + if (!i2c_send_start()) + return FALSE; + if (!i2c_send_address(0x52, TRUE)) + return FALSE; + if (!i2c_send_data(0x00)) + return FALSE; + i2c_send_stop(); + + if (!i2c_send_start()) + return FALSE; + if (!i2c_send_address(0x52, FALSE)) + return FALSE; + for (i = 0; i < 6; i++) { + if (!i2c_receive_data(&(b[i]), (i == 5))) + return FALSE; + } + i2c_send_stop(); + + wmp_yaw = ((b[3]>>2)<<8) + b[0]; + wmp_pitch = ((b[4]>>2)<<8) + b[1]; + wmp_roll = ((b[5]>>2)<<8) + b[2]; + + /* XXX We don't take into account the fast/slow mode flag here */ + wmp_yaw_fast = !(b[3] & 0x2); + wmp_pitch_fast = !(b[3] & 0x1); + wmp_roll_fast = !(b[4] & 0x2); + + return TRUE; +} + diff --git a/wmp.h b/wmp.h new file mode 100644 index 0000000..dd2243a --- /dev/null +++ b/wmp.h @@ -0,0 +1,20 @@ +#ifndef __WMP_H +#define __WMP_H + +#include "types.h" + +extern unsigned int wmp_yaw; +extern unsigned int wmp_pitch; +extern unsigned int wmp_roll; + +extern unsigned char wmp_calibration_data[]; + +extern bool wmp_yaw_fast; +extern bool wmp_pitch_fast; +extern bool wmp_roll_fast; + +bool wmp_init(void); +bool wmp_sample(void); +bool wmp_read_calibration_data(void); + +#endif /* __WMP_H */