rik/libwiiuse
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Changed the initial handshake to be synchronous

Browse Source 
Made the Motion+ init a lot more robust
Code cleanup
This commit is contained in:
Jan Ciger
2012-01-15 04:43:09 +01:00
committed by Ryan Pavlik
parent ce9e1b6156
commit 2e347bc523
6 changed files with 268 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/events.c
View File

@@ -164,6 +164,9 @@ int wiiuse_poll(struct wiimote_t** wm, int wiimotes) {
propagate_event(wm[i], wm[i]->event_buf[1], wm[i]->event_buf+2);
evnt += (wm[i]->event != WIIUSE_NONE);
} else {
/* send out any waiting writes */
wiiuse_send_next_pending_write_request(wm[i]);
idle_cycle(wm[i]);
}
}
@@ -186,6 +189,9 @@ int wiiuse_poll(struct wiimote_t** wm, int wiimotes) {
/* clear out the event buffer */
memset(wm[i]->event_buf, 0, sizeof(wm[i]->event_buf));
} else {
/* send out any waiting writes */
wiiuse_send_next_pending_write_request(wm[i]);
idle_cycle(wm[i]);
}
}
@@ -650,9 +656,16 @@ static void event_status(struct wiimote_t* wm, byte* msg) {
if (msg[2] & WM_CTRL_STATUS_BYTE1_LED_3) led[2] = 1;
if (msg[2] & WM_CTRL_STATUS_BYTE1_LED_4) led[3] = 1;
/* probe for Motion+ */
if(!WIIMOTE_IS_SET(wm, WIIMOTE_STATE_MPLUS_PRESENT))
wiiuse_probe_motion_plus(wm);
/* is an attachment connected to the expansion port? */
if ((msg[2] & WM_CTRL_STATUS_BYTE1_ATTACHMENT) == WM_CTRL_STATUS_BYTE1_ATTACHMENT)
{
WIIUSE_DEBUG("Attachment detected!");
attachment = 1;
}
/* is the speaker enabled? */
if ((msg[2] & WM_CTRL_STATUS_BYTE1_SPEAKER_ENABLED) == WM_CTRL_STATUS_BYTE1_SPEAKER_ENABLED)
@@ -708,7 +721,6 @@ static void event_status(struct wiimote_t* wm, byte* msg) {
req->state = REQ_DONE;
/* if(req->cb!=NULL) req->cb(wm,msg,6); */
free(req);
wiiuse_send_next_pending_write_request(wm);
}

159
src/io.c
View File

@@ -32,9 +32,11 @@
*/
#include "io.h"
#include "ir.h" /* for wiiuse_set_ir_mode */
#include "wiiuse_internal.h"
#include <stdlib.h> /* for free, malloc */
extern void propagate_event(struct wiimote_t* wm, byte event, byte* msg);
static void wiiuse_disable_motion_plus2(struct wiimote_t *wm,byte *data,unsigned short len)
{
@@ -53,6 +55,85 @@ static void wiiuse_disable_motion_plus1(struct wiimote_t *wm,byte *data,unsigned
wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE1, &val, 1, wiiuse_disable_motion_plus2);
}
/**
* @brief Wait until specified report arrives and return it
*
* @param wm Pointer to a wiimote_t structure.
* @param data Pre-allocated memory to store the received data
*
* Synchronous/blocking, this function will not return until it receives the specified
* report from the Wiimote.
*
*/
void wiiuse_wait_report(struct wiimote_t *wm, int report, byte *data)
{
for(;;)
{
if(wiiuse_io_read(wm, data, 32) > 0)
if(data[1] == report)
break;
}
}
/**
* @brief Read memory/register data synchronously
*
* @param wm Pointer to a wiimote_t structure.
* @param memory If set to non-zero, reads EEPROM, otherwise registers
* @param addr Address offset to read from
* @param size How many bytes to read
* @param data Pre-allocated memory to store the received data
*
* Synchronous/blocking read, this function will not return until it receives the specified
* amount of data from the Wiimote.
*
*/
void wiiuse_read(struct wiimote_t *wm, byte memory, unsigned addr, unsigned short size, byte *data)
{
byte pkt[8];
byte buf[32];
unsigned n_full_reports;
unsigned last_report;
byte *output;
int i;
/*
* address in big endian first, the leading byte will
* be overwritten (only 3 bytes are sent)
*/
to_big_endian_uint32_t(pkt + 2, addr);
pkt[0] = 0x52; /* HID read command */
pkt[1] = 0x17; /* report 17 - read */
pkt[2] = (memory != 0) ? 0x00 : 0x04; /* read from registers or memory*/
/*
* length in big endian
*/
to_big_endian_uint16_t(pkt + 6, size);
/* calculate how many 16B packets we have to get back */
n_full_reports = size / 16;
last_report = size % 16;
output = data;
wiiuse_io_write(wm, pkt, 8);
for(i = 0; i < n_full_reports; ++i)
{
wiiuse_wait_report(wm, 0x21, buf);
memmove(output, buf + 7, 16);
output += 16;
}
/* read the last incomplete packet */
if(last_report)
{
wiiuse_wait_report(wm, 0x21, buf);
memmove(output, buf + 7, last_report);
}
}
/**
* @brief Get initialization data from the wiimote.
*
@@ -66,18 +147,93 @@ static void wiiuse_disable_motion_plus1(struct wiimote_t *wm,byte *data,unsigned
* The handshake will be concluded when the wiimote responds
* with this data.
*/
#define SYNC_HANDSHAKE 1
#ifdef SYNC_HANDSHAKE
void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len)
{
/* send request to wiimote for accelerometer calibration */
byte buf[32];
byte report_type;
/* step 0 - Reset wiimote */
{
//wiiuse_set_leds(wm, WIIMOTE_LED_NONE);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_CONNECTED);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_ACC);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_RUMBLE);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP);
WIIMOTE_DISABLE_FLAG(wm, WIIUSE_CONTINUOUS);
wiiuse_set_report_type(wm);
wiiuse_millisleep(500);
WIIUSE_DEBUG("Wiimote reset!\n");
}
/* step 1 - calibration of accelerometers */
{
struct accel_t* accel = &wm->accel_calib;
byte val;
wiiuse_read(wm, 1, WM_MEM_OFFSET_CALIBRATION, 8, buf);
/* received read data */
accel->cal_zero.x = buf[0];
accel->cal_zero.y = buf[1];
accel->cal_zero.z = buf[2];
accel->cal_g.x = buf[4] - accel->cal_zero.x;
accel->cal_g.y = buf[5] - accel->cal_zero.y;
accel->cal_g.z = buf[6] - accel->cal_zero.z;
WIIUSE_DEBUG("Calibrated wiimote acc\n");
}
/* step 2 - re-enable IR and ask for status */
{
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
/* now enable IR if it was set before the handshake completed */
if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR))
{
WIIUSE_DEBUG("Handshake finished, enabling IR.");
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
wiiuse_set_ir(wm, 1);
}
WIIUSE_DEBUG("Asking for status ...\n");
wm->event = WIIUSE_CONNECT;
wiiuse_status(wm);
}
}
#else
void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) {
if (!wm) return;
switch (wm->handshake_state) {
case 0:
{
/* send request to wiimote for accelerometer calibration */
byte* buf;
/* continous reporting off, report to buttons only */
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE);
wiiuse_set_leds(wm, WIIMOTE_LED_NONE);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_ACC);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_RUMBLE);
WIIMOTE_DISABLE_FLAG(wm, WIIUSE_CONTINUOUS);
wiiuse_set_report_type(wm);
/* send request to wiimote for accelerometer calibration */
buf = (byte*)malloc(sizeof(byte) * 8);
wiiuse_read_data_cb(wm, wiiuse_handshake, buf, WM_MEM_OFFSET_CALIBRATION, 7);
wm->handshake_state++;
@@ -143,3 +299,4 @@ void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) {
}
}
}
#endif

5
src/io.h
View File

@@ -52,7 +52,10 @@ void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len);
void wiiuse_init_platform_fields(struct wiimote_t* wm);
void wiiuse_cleanup_platform_fields(struct wiimote_t* wm);
int wiiuse_io_read(struct wiimote_t* wm);
void wiiuse_wait_report(struct wiimote_t *wm, int report, byte *data);
void wiiuse_read(struct wiimote_t *wm, byte memory, unsigned addr, unsigned short size, byte *data);
int wiiuse_io_read(struct wiimote_t* wm, byte* buf, int len);
int wiiuse_io_write(struct wiimote_t* wm, byte* buf, int len);
/** @} */

20
src/io_nix.c
View File

@@ -267,14 +267,26 @@ void wiiuse_disconnect(struct wiimote_t* wm) {
}
int wiiuse_io_read(struct wiimote_t* wm) {
/* not used */
return 0;
int wiiuse_io_read(struct wiimote_t* wm, byte* buf, int len) {
int rc;
rc = read(wm->in_sock, buf, len);
if(rc <= 0)
wiiuse_disconnected(wm);
return rc;
}
int wiiuse_io_write(struct wiimote_t* wm, byte* buf, int len) {
return write(wm->out_sock, buf, len);
int rc;
rc = write(wm->out_sock, buf, len);
if(rc < 0)
wiiuse_disconnected(wm);
return rc;
}
void wiiuse_init_platform_fields(struct wiimote_t* wm) {

69
src/motion_plus.c
View File

@@ -30,6 +30,7 @@
#include "motion_plus.h"
#include "wiiuse_internal.h" /* for endian conversions */
#include "events.h" /* for disable_expansion */
#include "ir.h" /* for wiiuse_set_ir_mode */
#include "nunchuk.h" /* for nunchuk_pressed_buttons */
@@ -42,6 +43,68 @@ static void wiiuse_calibrate_motion_plus(struct motion_plus_t *mp);
static void calculate_gyro_rates(struct motion_plus_t* mp);
void wiiuse_probe_motion_plus(struct wiimote_t *wm)
{
byte buf[32];
unsigned id;
wiiuse_read(wm, 0, WM_EXP_MOTION_PLUS_IDENT, 6, buf);
/* check error code */
if(buf[4] & 0x0f)
{
WIIUSE_DEBUG("No Motion+ available, stopping probe.");
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_MPLUS_PRESENT);
return;
}
/* decode the id*/
id = from_big_endian_uint32_t(buf + 2);
if(id != EXP_ID_CODE_INACTIVE_MOTION_PLUS &&
id != EXP_ID_CODE_NLA_MOTION_PLUS &&
id != EXP_ID_CODE_NLA_MOTION_PLUS_NUNCHUK &&
id != EXP_ID_CODE_NLA_MOTION_PLUS_CLASSIC)
{
/* we have read something weird */
WIIUSE_DEBUG("Motion+ ID doesn't match, probably not connected.");
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_MPLUS_PRESENT);
return;
}
WIIUSE_DEBUG("Detected inactive Motion+!");
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_MPLUS_PRESENT);
/* init M+ */
buf[0] = 0x55;
wiiuse_write_data(wm, WM_EXP_MOTION_PLUS_INIT, buf, 1);
/* Init whatever is hanging on the pass-through port */
buf[0] = 0x55;
wiiuse_write_data(wm, WM_EXP_MEM_ENABLE1, buf, 1);
buf[0] = 0x00;
wiiuse_write_data(wm, WM_EXP_MEM_ENABLE2, buf, 1);
/* Init gyroscope data */
wm->exp.mp.cal_gyro.roll = 0;
wm->exp.mp.cal_gyro.pitch = 0;
wm->exp.mp.cal_gyro.yaw = 0;
wm->exp.mp.orient.roll = 0.0;
wm->exp.mp.orient.pitch = 0.0;
wm->exp.mp.orient.yaw = 0.0;
wm->exp.mp.raw_gyro_threshold = 10;
wm->exp.mp.nc = &(wm->exp.nunchuk);
wm->exp.mp.classic = &(wm->exp.classic);
wm->exp.nunchuk.flags = &wm->flags;
wm->exp.mp.ext = 0;
wiiuse_set_ir_mode(wm);
wiiuse_set_report_type(wm);
}
void wiiuse_motion_plus_handshake(struct wiimote_t *wm,byte *data,unsigned short len)
{
uint32_t val;
@@ -133,7 +196,11 @@ void wiiuse_set_motion_plus(struct wiimote_t *wm, int status)
{
byte val;
if(status && !WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP_HANDSHAKE))
if(!WIIMOTE_IS_SET(wm, WIIMOTE_STATE_MPLUS_PRESENT) ||
WIIMOTE_IS_SET(wm, WIIMOTE_STATE_EXP_HANDSHAKE))
return;
if(status)
{
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
val = (status == 1) ? 0x04 : 0x05;

10
src/wiiuse_internal.h
View File

@@ -172,6 +172,7 @@
#define WM_EXP_MEM_ENABLE1 0x04A400F0
#define WM_EXP_MEM_ENABLE2 0x04A400FB
#define WM_EXP_MEM_CALIBR 0x04A40020
#define WM_EXP_MOTION_PLUS_IDENT 0x04A600FA
#define WM_EXP_MOTION_PLUS_ENABLE 0x04A600FE
#define WM_EXP_MOTION_PLUS_INIT 0x04A600F0
#define WM_REG_IR 0x04B00030
@@ -209,10 +210,16 @@
#define EXP_ID_CODE_WII_BOARD 0xA4200402
#define EXP_ID_CODE_CLASSIC_CONTROLLER 0xA4200101
#define EXP_ID_CODE_GUITAR 0xA4200103
#define EXP_ID_CODE_MOTION_PLUS 0xa4200405
#define EXP_ID_CODE_MOTION_PLUS 0xA4200405
#define EXP_ID_CODE_MOTION_PLUS_NUNCHUK 0xA4200505 /** Motion Plus ID in Nunchuck passthrough mode */
#define EXP_ID_CODE_MOTION_PLUS_CLASSIC 0xA4200705 /** Motion Plus ID in Classic control. passthrough */
/* decrypted M+ codes at 0x04A600FA */
#define EXP_ID_CODE_INACTIVE_MOTION_PLUS 0xA6200005 /** Inactive Motion Plus ID */
#define EXP_ID_CODE_NLA_MOTION_PLUS 0xA6200405 /** No longer active Motion Plus ID */
#define EXP_ID_CODE_NLA_MOTION_PLUS_NUNCHUK 0xA6200505 /** No longer active Motion Plus ID in Nunchuck passthrough mode */
#define EXP_ID_CODE_NLA_MOTION_PLUS_CLASSIC 0xA6200705 /** No longer active Motion Plus ID in Classic control. passthrough */
#define EXP_HANDSHAKE_LEN 224
/********************
@@ -239,6 +246,7 @@
#define WIIMOTE_STATE_EXP_HANDSHAKE 0x10000 /* actual M+ connection exists but no handshake yet */
#define WIIMOTE_STATE_EXP_EXTERN 0x20000 /* actual M+ connection exists but handshake failed */
#define WIIMOTE_STATE_EXP_FAILED 0x40000 /* actual M+ connection exists but handshake failed */
#define WIIMOTE_STATE_MPLUS_PRESENT 0x80000 /* Motion+ is connected */