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

More hacking:

Browse Source 
* initial Wii Motion+ support
* initial timer support
This commit is contained in:
Gavan Fantom
2011-05-15 22:22:50 +00:00
parent 52e1c59a2e
commit b3f0bbf7c9
9 changed files with 415 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

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

36
i2c.c
View File

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

6
i2c.h
View File

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

224
main.c
View File

@@ -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,30 +277,60 @@ int main(void) {
else
reply("FAIL");
break;
case 'D':
putstr("Sending address... ");
if (i2c_send_address(0x53, TRUE))
reply("OK");
else
reply("FAIL");
break;
case 'B':
putstr("Sending bytes... ");
if (i2c_send_data(0xfe))
reply("OK");
else
reply("FAIL");
if (i2c_send_data(0x04))
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;

53
timer.c Normal file
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 Normal file
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
View File

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

100
wmp.c Normal file
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 Normal file
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 */