From 9ae7f37396428ac21249bad6469414dfb5840169 Mon Sep 17 00:00:00 2001 From: Jan Ciger Date: Wed, 14 Sep 2011 09:12:33 -0500 Subject: [PATCH] Added expansion detection from fWiine Added gyro rate calculations & calibrations from WiiC Conflicts: src/events.c src/io.c src/motion_plus.c src/wiiuse_internal.h --- src/events.c | 135 +++++++++++++++++++++++++++++++----------- src/io.c | 28 ++++++++- src/motion_plus.c | 17 ++++-- src/wiiuse_internal.h | 16 +++-- 4 files changed, 149 insertions(+), 47 deletions(-) diff --git a/src/events.c b/src/events.c index 5e67358..f4de7db 100644 --- a/src/events.c +++ b/src/events.c @@ -609,6 +609,10 @@ static void event_status(struct wiimote_t* wm, byte* msg) { int exp_changed = 0; struct data_req_t* req = wm->data_req; + /* initial handshake is not finished yet, ignore this */ + if(WIIMOTE_IS_SET(wm, WIIMOTE_STATE_HANDSHAKE)) + return; + /* * An event occured. * This event can be overwritten by a more specific @@ -661,7 +665,7 @@ static void event_status(struct wiimote_t* wm, byte* msg) { #if 0 switch(wm->expansion_state) { - case 0: // regular expansion detection + case 0: /* regular expansion detection */ { if(attachment && wm->exp.type != EXP_NONE) { @@ -671,7 +675,7 @@ static void event_status(struct wiimote_t* wm, byte* msg) { else { - // give it another chance still + /* give it another chance still */ if(wm->expansion_dattempts < 10) { wm->expansion_dattempts++; @@ -679,7 +683,7 @@ static void event_status(struct wiimote_t* wm, byte* msg) { return; } - // likely no attachment, give up and try M+ + /* likely no attachment, give up and try M+ */ else { wm->expansion_dattempts = 0; wiiuse_set_motion_plus(wm, WIIMOTE_IS_SET(wm, WIIMOTE_STATE_EXP) ? 2 : 1); @@ -690,7 +694,7 @@ static void event_status(struct wiimote_t* wm, byte* msg) { break; } - case 1: // try to init Motion+ + case 1: /* try to init Motion+ */ { if(attachment && wm->exp.type != EXP_NONE) { @@ -700,7 +704,7 @@ static void event_status(struct wiimote_t* wm, byte* msg) { else { - // give it another chance still + /* give it another chance still */ if(wm->expansion_dattempts < 10) { wm->expansion_dattempts++; @@ -709,7 +713,7 @@ static void event_status(struct wiimote_t* wm, byte* msg) { } else { - // give up and move on + /* give up and move on */ wm->expansion_state++; } } @@ -727,37 +731,27 @@ static void event_status(struct wiimote_t* wm, byte* msg) { * We need to send a WIIMOTE_CMD_REPORT_TYPE packet to * reenable other incoming reports. */ - if (exp_changed) - { if (exp_changed && WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR)) - { - /* - * Since the expansion status changed IR needs to - * be reset for the new IR report mode. - */ - WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR); - wiiuse_set_ir(wm, 1); - } else { - wiiuse_set_report_type(wm); - return; - } - - /* handling new Tx for changed exp */ - if(!req) return; - if(!(req->state==REQ_SENT)) return; - - wm->data_req = req->next; - - req->state = REQ_DONE; - /* if(req->cb!=NULL) req->cb(wm,msg,6); */ - - free(req); - wiiuse_send_next_pending_write_request(wm); - - } else { + /* + * Since the expansion status changed IR needs to + * be reset for the new IR report mode. + */ + WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR); + wiiuse_set_ir(wm, 1); + } else { wiiuse_set_report_type(wm); + return; } + + /* handling new Tx for changed exp */ + if(!req) return; + if(!(req->state==REQ_SENT)) return; + wm->data_req = req->next; + req->state = REQ_DONE; + /* if(req->cb!=NULL) req->cb(wm,msg,6); */ + free(req); + wiiuse_send_next_pending_write_request(wm); } @@ -807,7 +801,79 @@ static void handle_expansion(struct wiimote_t* wm, byte* msg) { */ void handshake_expansion(struct wiimote_t* wm, byte* data, uint16_t len) { int id; + byte val = 0; + byte buf = 0x00; + byte* handshake_buf; +#ifndef OLD_EXP_HANDSHAKE + switch(wm->expansion_state) { + /* These two initialization writes disable the encryption */ + case 0: + wm->expansion_state = 1; + /* increase the timeout until the handshake completes */ +#ifdef WIIUSE_WIN32 + WIIUSE_DEBUG("write 0x55 - Setting timeout to expansion %i ms.", wm->exp_timeout); + wm->timeout = wm->exp_timeout; +#endif + buf = 0x55; + wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE1, &buf, 1, handshake_expansion); + break; + case 1: + wm->expansion_state = 2; + /* increase the timeout until the handshake completes */ +#ifdef WIIUSE_WIN32 + WIIUSE_DEBUG("write 0x00 - Setting timeout to expansion %i ms.", wm->exp_timeout); + wm->timeout = wm->exp_timeout; +#endif + val = 0x00; + wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE2, &buf, 1, handshake_expansion); + break; + case 2: + wm->expansion_state = 3; + /* get the calibration data */ + if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_EXP)) + disable_expansion(wm); + handshake_buf = malloc(EXP_HANDSHAKE_LEN * sizeof(byte)); + wiiuse_read_data_cb(wm, handshake_expansion, handshake_buf, WM_EXP_MEM_CALIBR, EXP_HANDSHAKE_LEN); + + /* tell the wiimote to send expansion data */ + WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP); + break; + case 3: + if(!data || !len) return; + id = from_big_endian_uint32_t(data + 220); + switch(id) { + case EXP_ID_CODE_NUNCHUK: + if (nunchuk_handshake(wm, &wm->exp.nunchuk, data, len)) + wm->event = WIIUSE_NUNCHUK_INSERTED; + break; + case EXP_ID_CODE_CLASSIC_CONTROLLER: + if (classic_ctrl_handshake(wm, &wm->exp.classic, data, len)) + wm->event = WIIUSE_CLASSIC_CTRL_INSERTED; + break; + case EXP_ID_CODE_GUITAR: + if (guitar_hero_3_handshake(wm, &wm->exp.gh3, data, len)) + wm->event = WIIUSE_GUITAR_HERO_3_CTRL_INSERTED; + break; + case EXP_ID_CODE_MOTION_PLUS: + case EXP_ID_CODE_MOTION_PLUS_CLASSIC: + case EXP_ID_CODE_MOTION_PLUS_NUNCHUK: + /* wiiuse_motion_plus_handshake(wm, data, len); */ + wm->event = WIIUSE_MOTION_PLUS_ACTIVATED; + break; + default: + WIIUSE_WARNING("Unknown expansion type. Code: 0x%x", id); + break; + } + free(data); + WIIMOTE_DISABLE_STATE(wm,WIIMOTE_STATE_EXP_HANDSHAKE); + WIIMOTE_ENABLE_STATE(wm,WIIMOTE_STATE_EXP); + wiiuse_set_ir_mode(wm); + wiiuse_status(wm); + break; + } + +#else if (!data) { byte* handshake_buf; byte buf = 0x00; @@ -865,7 +931,7 @@ void handshake_expansion(struct wiimote_t* wm, byte* data, uint16_t len) { case EXP_ID_CODE_MOTION_PLUS: case EXP_ID_CODE_MOTION_PLUS_CLASSIC: case EXP_ID_CODE_MOTION_PLUS_NUNCHUK: - //wiiuse_motion_plus_handshake(wm, data, len); + /* wiiuse_motion_plus_handshake(wm, data, len); */ wm->event = WIIUSE_MOTION_PLUS_ACTIVATED; break; @@ -878,6 +944,7 @@ void handshake_expansion(struct wiimote_t* wm, byte* data, uint16_t len) { } free(data); +#endif } diff --git a/src/io.c b/src/io.c index 5bd9e33..6cde801 100644 --- a/src/io.c +++ b/src/io.c @@ -35,6 +35,23 @@ #include /* for free, malloc */ +static void wiiuse_disable_motion_plus2(struct wiimote_t *wm,byte *data,unsigned short len) +{ + WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_FAILED); + WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE); + wiiuse_set_ir_mode(wm); + + wm->handshake_state++; + wiiuse_handshake(wm, NULL, 0); + +} + +static void wiiuse_disable_motion_plus1(struct wiimote_t *wm,byte *data,unsigned short len) +{ + byte val = 0x00; + wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE1, &val, 1, wiiuse_disable_motion_plus2); +} + /** * @brief Get initialization data from the wiimote. * @@ -91,11 +108,16 @@ void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) { accel->cal_zero.x, accel->cal_zero.y, accel->cal_zero.z, accel->cal_g.x, accel->cal_g.y, accel->cal_g.z); - /*wiiuse_set_motion_plus(wm, 0);*/ - wiiuse_status(wm); + /* M+ off */ + byte val = 0x55; + wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE1, &val, 1, wiiuse_disable_motion_plus1); + break; + } + + case 2: + { /* request the status of the wiimote to check for any expansion */ - wiiuse_status(wm); WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE); WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE); wm->handshake_state++; diff --git a/src/motion_plus.c b/src/motion_plus.c index d79208c..37eaeb2 100644 --- a/src/motion_plus.c +++ b/src/motion_plus.c @@ -53,6 +53,7 @@ void wiiuse_motion_plus_handshake(struct wiimote_t *wm,byte *data,unsigned short { WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_FAILED); WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE); + WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP); /* tell wiimote to include exp. data in reports */ val = from_big_endian_uint32_t(data + 2); @@ -103,6 +104,7 @@ void wiiuse_motion_plus_handshake(struct wiimote_t *wm,byte *data,unsigned short wm->exp.mp.ext_initialized = 0; wiiuse_set_ir_mode(wm); + wiiuse_set_report_type(wm); } } } @@ -129,7 +131,7 @@ void wiiuse_set_motion_plus(struct wiimote_t *wm, int status) { WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE); val = (status == 1) ? 0x04 : 0x05; - wiiuse_write_data_cb(wm, WM_EXP_MOTION_PLUS_ENABLE, &val, 1, wiiuse_motion_plus_check); + wiiuse_write_data_cb(wm, WM_EXP_MOTION_PLUS_ENABLE, &val, 1, wiiuse_motion_plus_handshake); } else { @@ -165,10 +167,15 @@ void motion_plus_event(struct motion_plus_t* mp, int exp_type, byte* msg) mp->raw_gyro.y = ((msg[3] & 0xFC) << 6) | msg[0]; /* First calibration */ - if ((mp->raw_gyro.r > 5000) && (mp->raw_gyro.p > 5000) && (mp->raw_gyro.y > 5000) && - !(mp->cal_gyro.r) - && !(mp->cal_gyro.p) - && !(mp->cal_gyro.y)) + if ((mp->raw_gyro.r > 5000) && + (mp->raw_gyro.p > 5000) && + (mp->raw_gyro.y > 5000) && + (mp->raw_gyro.r < 0x3fff) && + (mp->raw_gyro.p < 0x3fff) && + (mp->raw_gyro.y < 0x3fff) && + !(mp->cal_gyro.r) && + !(mp->cal_gyro.p) && + !(mp->cal_gyro.y)) { wiiuse_calibrate_motion_plus(mp); } diff --git a/src/wiiuse_internal.h b/src/wiiuse_internal.h index ef05501..4f6de90 100644 --- a/src/wiiuse_internal.h +++ b/src/wiiuse_internal.h @@ -167,11 +167,17 @@ */ /* encrypted expansion id codes (located at 0x04A400FC) */ -#define EXP_ID_CODE_NUNCHUK 0x9A1EFEFE -#define EXP_ID_CODE_WII_BOARD 0xA4200402 -#define EXP_ID_CODE_CLASSIC_CONTROLLER 0x9A1EFDFD -#define EXP_ID_CODE_GUITAR 0x9A1EFDFB -#define EXP_ID_CODE_MOTION_PLUS 0xa4200405 +/* #define EXP_ID_CODE_NUNCHUK 0x9A1EFEFE */ +/* #define EXP_ID_CODE_WII_BOARD 0xA4200402 */ +/* #define EXP_ID_CODE_CLASSIC_CONTROLLER 0x9A1EFDFD */ +/* #define EXP_ID_CODE_GUITAR 0x9A1EFDFB */ + +/* decrypted expansion id codes (located at 0x04A400FC) */ +#define EXP_ID_CODE_NUNCHUK 0xA4200000 +#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_NUNCHUK 0xA4200505 /** Motion Plus ID in Nunchuck passthrough mode */ #define EXP_ID_CODE_MOTION_PLUS_CLASSIC 0xA4200705 /** Motion Plus ID in Classic control. passthrough */