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.
205 lines
3.4 KiB
205 lines
3.4 KiB
#include <windows.h> |
|
#include <stdio.h> |
|
#include "ioport.h" |
|
|
|
/* |
|
* This is a hacked up piece of bit-banging code to query a Wii Motion Plus |
|
* over I2C on the parallel port. It's not polished code by a long way. |
|
* You have been warned. |
|
*/ |
|
|
|
/* This is the first parallel port on most PCs. */ |
|
#define PAR 0x378 |
|
|
|
#define SCL_MASK 0x40 |
|
#define SDA_MASK 0x80 |
|
#define SDA_IN_MASK 0x40 |
|
|
|
#if 0 |
|
#define DELAY Sleep(1) |
|
#else |
|
int _delay; |
|
#define DELAY for (_delay = 0; _delay < DELAY_COUNT; _delay++) {} |
|
#endif |
|
|
|
/* |
|
* Carefully Calibrated[tm] delay loop. |
|
* This works on my PC, it may not work on yours. |
|
* You have been warned. |
|
*/ |
|
#define DELAY_COUNT 10000 |
|
|
|
#define NCAL 100 |
|
|
|
int roll0, pitch0, yaw0; |
|
|
|
int paroutvalue = SCL_MASK | SDA_MASK; |
|
|
|
void initparallel(void) |
|
{ |
|
paroutvalue = SCL_MASK | SDA_MASK; |
|
outportb(PAR+2, 0x20); |
|
outportb(PAR+0, paroutvalue); |
|
DELAY; |
|
} |
|
|
|
void setscl(int value) |
|
{ |
|
/* We have an inverter in the way */ |
|
value = !value; |
|
|
|
paroutvalue &= ~SCL_MASK; |
|
paroutvalue |= (value ? SCL_MASK : 0); |
|
outportb(PAR+0, paroutvalue); |
|
DELAY; |
|
} |
|
|
|
void setsda(int value) |
|
{ |
|
/* We have an inverter in the way */ |
|
value = !value; |
|
|
|
paroutvalue &= ~SDA_MASK; |
|
paroutvalue |= (value ? SDA_MASK : 0); |
|
outportb(PAR+0, paroutvalue); |
|
DELAY; |
|
} |
|
|
|
int getsda(void) |
|
{ |
|
int value; |
|
|
|
value = inportb(PAR+1) & SDA_IN_MASK; |
|
|
|
/* We have an inverter in the way */ |
|
value = !value; |
|
|
|
return (value != 0); |
|
} |
|
|
|
void send_start(void) |
|
{ |
|
setsda(1); |
|
setscl(1); |
|
setsda(0); |
|
} |
|
|
|
void send_stop(void) |
|
{ |
|
setsda(0); |
|
setscl(1); |
|
setsda(1); |
|
} |
|
|
|
int send_byte(int value) |
|
{ |
|
int i; |
|
int ack; |
|
|
|
for (i = 0; i < 8; i++) { |
|
setscl(0); |
|
setsda(value & 0x80); |
|
setscl(1); |
|
value = value << 1; |
|
} |
|
setscl(0); |
|
setsda(1); |
|
setscl(1); |
|
ack = !getsda(); |
|
setscl(0); |
|
if (!ack) { |
|
send_stop(); |
|
printf("ACK not received\n"); |
|
return FALSE; |
|
} |
|
return TRUE; |
|
} |
|
|
|
int get_byte(int last) |
|
{ |
|
int i; |
|
int value = 0; |
|
|
|
setsda(1); |
|
|
|
for (i = 0; i < 8; i++) { |
|
setscl(0); |
|
setscl(1); |
|
value = value << 1; |
|
value |= (getsda()?1:0); |
|
} |
|
setscl(0); |
|
if (!last) |
|
setsda(0); |
|
setscl(1); |
|
setscl(0); |
|
setsda(1); |
|
|
|
return value; |
|
} |
|
|
|
void setupi2c(void) |
|
{ |
|
send_start(); |
|
send_byte((0x53 << 1) + 0); /* Address 0x53, write */ |
|
send_byte(0xfe); |
|
send_byte(0x04); |
|
send_stop(); |
|
} |
|
|
|
void get_readings(int *b) |
|
{ |
|
send_start(); |
|
send_byte((0x52 << 1) + 0); /* Address 0x52, write */ |
|
send_byte(0); |
|
send_stop(); |
|
|
|
send_start(); |
|
send_byte((0x52 << 1) + 1); /* Address 0x52, read */ |
|
b[0] = get_byte(0); |
|
b[1] = get_byte(0); |
|
b[2] = get_byte(0); |
|
b[3] = get_byte(0); |
|
b[4] = get_byte(0); |
|
b[5] = get_byte(1); |
|
send_stop(); |
|
} |
|
|
|
void query_device(int *roll, int *pitch, int *yaw) |
|
{ |
|
int b[6]; |
|
|
|
get_readings(b); |
|
|
|
*yaw = ((b[3]>>2)<<8) + b[0] - yaw0; |
|
*pitch = ((b[4]>>2)<<8) + b[1] - pitch0; |
|
*roll = ((b[5]>>2)<<8) + b[2] - roll0; |
|
} |
|
|
|
void calibrate(void) |
|
{ |
|
int b[6]; |
|
int i; |
|
|
|
yaw0 = 0; |
|
pitch0 = 0; |
|
roll0 = 0; |
|
|
|
printf("Calibrating... "); |
|
|
|
for (i = 0; i < NCAL; i++) { |
|
printf("%2d", i); |
|
get_readings(b); |
|
yaw0 += ((b[3]>>2)<<8) + b[0]; |
|
pitch0 += ((b[4]>>2)<<8) + b[1]; |
|
roll0 += ((b[5]>>2)<<8) + b[2]; |
|
printf("\b\b"); |
|
} |
|
|
|
yaw0 = yaw0 / NCAL; |
|
pitch0 = pitch0 / NCAL; |
|
roll0 = roll0 / NCAL; |
|
|
|
printf("done\n"); |
|
printf("Calibrated values: %d, %d, %d\n", roll0, pitch0, yaw0); |
|
}
|
|
|