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

More hacking: 

* initial Wii Motion+ support
* initial timer support
master
Gavan Fantom 14 years ago
parent
52e1c59a2e
commit
b3f0bbf7c9
9 changed files with 413 additions and 47 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      Makefile
  2. 36
      i2c.c
  3. 6
      i2c.h
  4. 220
      main.c
  5. 53
      timer.c
  6. 18
      timer.h
  7. 5
      types.h
  8. 100
      wmp.c
  9. 20
      wmp.h

2
Makefile
Unescape View File

@ -3,7 +3,7 @@
NAME=quad NAME=quad
SSRCS=crt0.s SSRCS=crt0.s
CSRCS=main.c i2c.c CSRCS=main.c i2c.c wmp.c timer.c
CFLAGS=-march=armv4t -msoft-float -O1 CFLAGS=-march=armv4t -msoft-float -O1

36
i2c.c
Unescape View File

@ -29,8 +29,13 @@ void init_i2c(void)
IWREG(I2SCLL) = (25 * 100); IWREG(I2SCLL) = (25 * 100);
IWREG(I2SCLH) = (12 * 100); IWREG(I2SCLH) = (12 * 100);
#else #else
# if 0
IWREG(I2SCLL) = 1475; /* ~5kHz */ IWREG(I2SCLL) = 1475; /* ~5kHz */
IWREG(I2SCLH) = 1475; IWREG(I2SCLH) = 1475;
# else
IWREG(I2SCLL) = 73; /* ~100kHz */
IWREG(I2SCLH) = 73;
# endif
#endif #endif
} }
@ -45,11 +50,6 @@ int i2c_wait(void)
return stat; return stat;
} }
void i2c_go(void)
{
IREG(I2CONCLR) = SIFLAG;
}
int i2c_conreg(void) int i2c_conreg(void)
{ {
return IREG(I2CONSET); return IREG(I2CONSET);
@ -64,14 +64,12 @@ bool i2c_send_start(void)
{ {
IREG(I2CONCLR) = STOFLAG | STAFLAG; IREG(I2CONCLR) = STOFLAG | STAFLAG;
IREG(I2CONSET) = STAFLAG; IREG(I2CONSET) = STAFLAG;
/* i2c_go(); */
switch (i2c_wait()) { switch (i2c_wait()) {
case 0x08: case 0x08:
case 0x10: case 0x10:
/* IREG(I2CONCLR) = STAFLAG; */
return TRUE; return TRUE;
default: default:
/* IREG(I2CONCLR) = STAFLAG; */ i2c_send_stop();
return FALSE; return FALSE;
} }
} }
@ -84,12 +82,30 @@ bool i2c_send_address(int addr, bool write)
bool i2c_send_data(int data) bool i2c_send_data(int data)
{ {
IREG(I2DAT) = data; IREG(I2DAT) = data;
IREG(I2CONCLR) = STAFLAG | SIFLAG; IREG(I2CONCLR) = STAFLAG | STOFLAG | SIFLAG;
switch (i2c_wait()) { switch (i2c_wait()) {
case 0x18: case 0x18:
case 0x28: 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; return TRUE;
default: default:
i2c_send_stop();
return FALSE; return FALSE;
} }
} }
@ -98,7 +114,7 @@ void i2c_send_stop(void)
{ {
IREG(I2CONCLR) = STAFLAG | AAFLAG; IREG(I2CONCLR) = STAFLAG | AAFLAG;
IREG(I2CONSET) = STOFLAG; IREG(I2CONSET) = STOFLAG;
i2c_go(); IREG(I2CONCLR) = SIFLAG;
/* Don't think we need to wait here. Could be wrong. */ /* Don't think we need to wait here. Could be wrong. */
} }

6
i2c.h
Unescape View File

@ -1,3 +1,6 @@
#ifndef __I2C_H
#define __I2C_H
#include "types.h" #include "types.h"
void init_i2c(void); void init_i2c(void);
extern int i2cstat; extern int i2cstat;
@ -8,4 +11,7 @@ int i2c_statreg(void);
bool i2c_send_start(void); bool i2c_send_start(void);
bool i2c_send_address(int addr, bool write); bool i2c_send_address(int addr, bool write);
bool i2c_send_data(int data); bool i2c_send_data(int data);
bool i2c_receive_data(int *data, bool last);
void i2c_send_stop(void); void i2c_send_stop(void);
#endif /* __I2C_H */

220
main.c
Unescape View File

@ -1,5 +1,7 @@
#include "wmp.h"
#include "i2c.h" #include "i2c.h"
#include "timer.h"
#define UARTBASE 0xE000C000 #define UARTBASE 0xE000C000
@ -23,20 +25,6 @@
#define U0THRE ((UREG(LSR) & (1<<5))) /* UART0 transmitter holding register is empty */ #define U0THRE ((UREG(LSR) & (1<<5))) /* UART0 transmitter holding register is empty */
#define U0DR ((UREG(LSR) & (1<<0))) /* UART0 data ready */ #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)) #define PINSEL0 (*((volatile unsigned char *) 0xE002C000))
void init_uart(void) void init_uart(void)
@ -109,10 +97,142 @@ void reply(char *str)
unsigned int count = 0; 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 main(void) {
int i;
int data;
init_uart(); init_uart();
init_i2c(); init_i2c();
init_pins(); init_pins();
init_timer();
putstr("Your entire life has been a mathematical error... a mathematical error I'm about to correct!\r\n"); putstr("Your entire life has been a mathematical error... a mathematical error I'm about to correct!\r\n");
while (1) { while (1) {
@ -140,17 +260,15 @@ int main(void) {
case '?': case '?':
reply("Help is not available. Try a psychiatrist."); reply("Help is not available. Try a psychiatrist.");
break; break;
case 'R':
putstr("I2C register is: ");
puthex(i2c_conreg());
reply(".");
break;
case 'T': case 'T':
putstr("I2C status was: "); putstr("I2C status was: ");
puthex(i2cstat); puthex(i2cstat);
putstr(" I2C status is: "); putstr(" I2C status is: ");
puthex(i2c_statreg()); puthex(i2c_statreg());
reply("."); reply(".");
putstr("I2C register is: ");
puthex(i2c_conreg());
reply(".");
break; break;
case 'S': case 'S':
putstr("Sending START... "); putstr("Sending START... ");
@ -159,29 +277,59 @@ int main(void) {
else else
reply("FAIL"); reply("FAIL");
break; break;
case 'D': case 'O':
putstr("Sending address... "); putstr("Sending STOP... ");
if (i2c_send_address(0x53, TRUE)) i2c_send_stop();
reply("OK"); reply("sent");
break;
case 'I':
putstr("Initialising WMP... ");
if (wmp_init())
reply("done");
else else
reply("FAIL"); reply("FAIL");
break; break;
case 'B': case 'M':
putstr("Sending bytes... "); putstr("Reading from WMP... ");
if (i2c_send_data(0xfe)) if (wmp_sample()) {
reply("OK"); putstr("(");
else puthex(wmp_roll);
putstr(", ");
puthex(wmp_pitch);
putstr(", ");
puthex(wmp_yaw);
reply(").");
} else
reply("FAIL"); reply("FAIL");
break;
if (i2c_send_data(0x04)) case 'L':
reply("OK"); minmax_sample();
else 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"); reply("FAIL");
}
break; break;
case 'O': case 'N':
putstr("Sending STOP... "); putstr("The time is ");
i2c_send_stop(); puthex(timer_read());
reply("sent"); reply(".");
break; break;
default: default:
reply("Unrecognised command."); reply("Unrecognised command.");

53
timer.c
Unescape View File

@ -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);
}

18
timer.h
Unescape View File

@ -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 */

5
types.h
Unescape View File

@ -1,3 +1,8 @@
#ifndef __TYPES_H
#define __TYPES_H
typedef int bool; typedef int bool;
#define TRUE 1 #define TRUE 1
#define FALSE 0 #define FALSE 0
#endif /* __TYPES_H */

100
wmp.c
Unescape View File

@ -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;
}

20
wmp.h
Unescape View File

@ -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 */
Write Preview
Loading…
Cancel
Save