diff --git a/CHANGELOG.mkd b/CHANGELOG.mkd index c132f52..426e21a 100644 --- a/CHANGELOG.mkd +++ b/CHANGELOG.mkd @@ -23,6 +23,9 @@ Added: client app as well as choosing the appropriate option when building WiiUse. The default is still a shared library (dll/so). Merged from paulburton and added to build system. +- Initial MotionPlus support. Thanks to admiral0 for the initial pull + request merging from fwiine, and to Jan Ciger Reviatech SAS for building + on it with fwiine and WiiC code as well as additional code. Fixed: @@ -31,6 +34,9 @@ Fixed: - Builds properly now on mingw (both cross-compile and native). +- Improved reliability on Windows when running an app twice without dis-connecting + the Wiimote (came as a part of the MotionPlus support.) + Changed: - Improved header includes using feedback from (include-what-you-use)[iwyu] diff --git a/README.mkd b/README.mkd index a5f3f50..767c693 100644 --- a/README.mkd +++ b/README.mkd @@ -53,7 +53,9 @@ Additional Contributors: - Karl Semich - Johannes Zarl - hartsantler +- admiral0 and fwiine project - Jeff Baker/Inv3rsion, LLC. +- Jan Ciger - Reviatech SAS License @@ -170,6 +172,12 @@ Known Issues On Windows using more than one wiimote (usually more than two wiimotes) may cause significant latency. +If you are going to use Motion+, make sure to call wiiuse_poll or wiiuse_update +in a loop for some 10-15 seconds before enabling it. Ideally you should be checking +the status of any expansion (nunchuk) you may have connected as well. +Otherwise the extra expansion may not initialize correctly - the initialization +and calibration takes some time. + Acknowledgements by Michael Laforest ------------------------------------ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index b4572b7..6eb4ea7 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -13,6 +13,8 @@ set(SOURCES dynamics.h events.h guitar_hero_3.h + motion_plus.h + motion_plus.c io.h ir.h nunchuk.h diff --git a/src/events.c b/src/events.c index 8a895ad..61b1605 100644 --- a/src/events.c +++ b/src/events.c @@ -43,6 +43,7 @@ #include "ir.h" /* for calculate_basic_ir, etc */ #include "nunchuk.h" /* for nunchuk_disconnected, etc */ #include "wiiboard.h" /* for wii_board_disconnected, etc */ +#include "motion_plus.h" /* for motion_plus_disconnected, etc */ #include "io.h" /* for wiiuse_io_read on Windows, etc */ #ifndef WIIUSE_WIN32 @@ -60,6 +61,7 @@ static void idle_cycle(struct wiimote_t* wm); static void clear_dirty_reads(struct wiimote_t* wm); static void propagate_event(struct wiimote_t* wm, byte event, byte* msg); static void event_data_read(struct wiimote_t* wm, byte* msg); +static void event_data_write(struct wiimote_t *wm, byte *msg); static void event_status(struct wiimote_t* wm, byte* msg); static void handle_expansion(struct wiimote_t* wm, byte* msg); @@ -396,7 +398,7 @@ static void propagate_event(struct wiimote_t* wm, byte event, byte* msg) { } case WM_RPT_WRITE: { - /* write feedback - safe to skip */ + event_data_write(wm,msg); break; } default: @@ -546,6 +548,52 @@ static void event_data_read(struct wiimote_t* wm, byte* msg) { } +static void event_data_write(struct wiimote_t *wm, byte *msg) +{ + + struct data_req_t* req = wm->data_req; + + wiiuse_pressed_buttons(wm,msg); + + /* if we don't have a request out then we didn't ask for this packet */ + if (!req) { + WIIUSE_WARNING("Transmitting data packet when no request was made."); + return; + } + if(!(req->state==REQ_SENT)) { + WIIUSE_WARNING("Transmission is not necessary"); + /* delete this request */ + wm->data_req = req->next; + free(req); + return; + } + + + req->state = REQ_DONE; + + if(req->cb) { + /* this was a callback, so invoke it now */ + req->cb(wm,NULL,0); + /* delete this request */ + wm->data_req = req->next; + free(req); + } else { + /* + * This should generate an event. + * We need to leave the event in the array so the client + * can access it still. We'll flag is as being 'REQ_DONE' + * and give the client one cycle to use it. Next event + * we will remove it from the list. + */ + wm->event = WIIUSE_WRITE_DATA; + + } + /* if another request exists send it to the wiimote */ + if (wm->data_req) { + wiiuse_send_next_pending_write_request(wm); + } +} + /** * @brief Read the controller status. * @@ -559,6 +607,11 @@ static void event_status(struct wiimote_t* wm, byte* msg) { int attachment = 0; int ir = 0; 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. @@ -613,15 +666,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 && WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR)) { + 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. + * 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 + } 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); } @@ -645,6 +710,11 @@ static void handle_expansion(struct wiimote_t* wm, byte* msg) { case EXP_WII_BOARD: wii_board_event(&wm->exp.wb, msg); break; + case EXP_MOTION_PLUS: + case EXP_MOTION_PLUS_CLASSIC: + case EXP_MOTION_PLUS_NUNCHUK: + motion_plus_event(&wm->exp.mp, wm->exp.type, msg); + break; default: break; } @@ -666,68 +736,76 @@ 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; - if (!data) { - byte* handshake_buf; - byte buf = 0x00; + 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); - if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_EXP)) - disable_expansion(wm); - - /* increase the timeout until the handshake completes */ - #ifdef WIIUSE_WIN32 - WIIUSE_DEBUG("Setting timeout to expansion %i ms.", wm->exp_timeout); - wm->timeout = wm->exp_timeout; - #endif - - wiiuse_write_data(wm, WM_EXP_MEM_ENABLE, &buf, 1); - - /* get the calibration data */ - handshake_buf = (byte *)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); - - return; + /* 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; } - - id = from_big_endian_uint32_t(data + 220); - - /* call the corresponding handshake function for this expansion */ - 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_WII_BOARD: - { - if (wii_board_handshake(wm, &wm->exp.wb, data, len)) - wm->event = WIIUSE_WII_BOARD_CTRL_INSERTED; - break; - } - default: - { - WIIUSE_WARNING("Unknown expansion type. Code: 0x%x", id); - break; - } - } - - free(data); } @@ -764,6 +842,12 @@ void disable_expansion(struct wiimote_t* wm) { wii_board_disconnected(&wm->exp.wb); wm->event = WIIUSE_WII_BOARD_CTRL_REMOVED; break; + case EXP_MOTION_PLUS: + case EXP_MOTION_PLUS_CLASSIC: + case EXP_MOTION_PLUS_NUNCHUK: + motion_plus_disconnected(&wm->exp.mp); + wm->event = WIIUSE_MOTION_PLUS_REMOVED; + break; default: break; } @@ -822,6 +906,33 @@ static void save_state(struct wiimote_t* wm) { wm->lstate.exp_wb_rbl = wm->exp.wb.rbl; break; + case EXP_MOTION_PLUS: + case EXP_MOTION_PLUS_CLASSIC: + case EXP_MOTION_PLUS_NUNCHUK: + { + wm->lstate.drx = wm->exp.mp.raw_gyro.pitch; + wm->lstate.dry = wm->exp.mp.raw_gyro.roll; + wm->lstate.drz = wm->exp.mp.raw_gyro.yaw; + + if(wm->exp.type == EXP_MOTION_PLUS_CLASSIC) + { + wm->lstate.exp_ljs_ang = wm->exp.classic.ljs.ang; + wm->lstate.exp_ljs_mag = wm->exp.classic.ljs.mag; + wm->lstate.exp_rjs_ang = wm->exp.classic.rjs.ang; + wm->lstate.exp_rjs_mag = wm->exp.classic.rjs.mag; + wm->lstate.exp_r_shoulder = wm->exp.classic.r_shoulder; + wm->lstate.exp_l_shoulder = wm->exp.classic.l_shoulder; + wm->lstate.exp_btns = wm->exp.classic.btns; + } else { + wm->lstate.exp_ljs_ang = wm->exp.nunchuk.js.ang; + wm->lstate.exp_ljs_mag = wm->exp.nunchuk.js.mag; + wm->lstate.exp_btns = wm->exp.nunchuk.btns; + wm->lstate.exp_accel = wm->exp.nunchuk.accel; + } + + break; + } + case EXP_NONE: break; } @@ -925,6 +1036,35 @@ static int state_changed(struct wiimote_t* wm) { STATE_CHANGED(wm->lstate.exp_wb_rbl,wm->exp.wb.bl); break; } + + case EXP_MOTION_PLUS: + case EXP_MOTION_PLUS_CLASSIC: + case EXP_MOTION_PLUS_NUNCHUK: + { + STATE_CHANGED(wm->lstate.drx, wm->exp.mp.raw_gyro.pitch); + STATE_CHANGED(wm->lstate.dry, wm->exp.mp.raw_gyro.roll); + STATE_CHANGED(wm->lstate.drz, wm->exp.mp.raw_gyro.yaw); + + if(wm->exp.type == EXP_MOTION_PLUS_CLASSIC) + { + STATE_CHANGED(wm->lstate.exp_ljs_ang, wm->exp.classic.ljs.ang); + STATE_CHANGED(wm->lstate.exp_ljs_mag, wm->exp.classic.ljs.mag); + STATE_CHANGED(wm->lstate.exp_rjs_ang, wm->exp.classic.rjs.ang); + STATE_CHANGED(wm->lstate.exp_rjs_mag, wm->exp.classic.rjs.mag); + STATE_CHANGED(wm->lstate.exp_r_shoulder, wm->exp.classic.r_shoulder); + STATE_CHANGED(wm->lstate.exp_l_shoulder, wm->exp.classic.l_shoulder); + STATE_CHANGED(wm->lstate.exp_btns, wm->exp.classic.btns); + } else { + STATE_CHANGED(wm->lstate.exp_ljs_ang, wm->exp.nunchuk.js.ang); + STATE_CHANGED(wm->lstate.exp_ljs_mag, wm->exp.nunchuk.js.mag); + STATE_CHANGED(wm->lstate.exp_btns, wm->exp.nunchuk.btns); + + CROSS_THRESH(wm->lstate.exp_orient, wm->exp.nunchuk.orient, wm->exp.nunchuk.orient_threshold); + CROSS_THRESH_XYZ(wm->lstate.exp_accel, wm->exp.nunchuk.accel, wm->exp.nunchuk.accel_threshold); + } + + break; + } case EXP_NONE: { break; diff --git a/src/io.c b/src/io.c index 44d2c36..168ff9f 100644 --- a/src/io.c +++ b/src/io.c @@ -31,10 +31,28 @@ * @brief Handles device I/O (non-OS specific). */ #include "io.h" +#include "ir.h" /* for wiiuse_set_ir_mode */ #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. * @@ -68,10 +86,12 @@ void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) { break; } + case 1: { struct read_req_t* req = wm->read_req; struct accel_t* accel = &wm->accel_calib; + byte val; /* received read data */ accel->cal_zero.x = req->buf[0]; @@ -90,10 +110,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); + /* M+ off */ + val = 0x55; + wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE1, &val, 1, wiiuse_disable_motion_plus1); - /* request the status of the wiimote to see if there is an expansion */ - wiiuse_status(wm); + break; + } + case 2: + { + /* request the status of the wiimote to check for any expansion */ WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE); WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE); wm->handshake_state++; @@ -105,8 +131,12 @@ void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) { wiiuse_set_ir(wm, 1); } + wm->event = WIIUSE_CONNECT; + wiiuse_status(wm); + break; } + default: { break; diff --git a/src/ir.c b/src/ir.c index b8f409f..8c6e4cd 100644 --- a/src/ir.c +++ b/src/ir.c @@ -57,6 +57,17 @@ static const byte WM_IR_BLOCK2_LEVEL4[] = "\x35\x03"; static const byte WM_IR_BLOCK1_LEVEL5[] = "\x07\x00\x00\x71\x01\x00\x72\x00\x20"; static const byte WM_IR_BLOCK2_LEVEL5[] = "\x1f\x03"; +void wiiuse_set_ir_mode(struct wiimote_t *wm) +{ + byte buf = 0x00; + + if(!wm) return; + if(!WIIMOTE_IS_SET(wm,WIIMOTE_STATE_IR)) return; + + if(WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP)) buf = WM_IR_TYPE_BASIC; + else buf = WM_IR_TYPE_EXTENDED; + wiiuse_write_data(wm,WM_REG_IR_MODENUM, &buf, 1); +} /** * @brief Set if the wiimote should track IR targets. * diff --git a/src/motion_plus.c b/src/motion_plus.c new file mode 100644 index 0000000..27d0d0e --- /dev/null +++ b/src/motion_plus.c @@ -0,0 +1,289 @@ +/* + * wiiuse + * + * Written By: + * Michal Wiedenbauer < shagkur > + * Dave Murphy < WinterMute > + * Hector Martin < marcan > + * Radu Andries + * + * Copyright 2009 + * + * This file is part of wiiuse and fWIIne. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * $Header$ + * + */ + +#include "motion_plus.h" + +#include "events.h" /* for disable_expansion */ +#include "ir.h" /* for wiiuse_set_ir_mode */ +#include "nunchuk.h" /* for nunchuk_pressed_buttons */ +#include "dynamics.h" /* for calc_joystick_state, etc */ + +#include /* for memset */ +#include /* for fabs */ + +static void wiiuse_calibrate_motion_plus(struct motion_plus_t *mp); +static void calculate_gyro_rates(struct motion_plus_t* mp); + + +void wiiuse_motion_plus_handshake(struct wiimote_t *wm,byte *data,unsigned short len) +{ + uint32_t val; + if(data == NULL) + { + wiiuse_read_data_cb(wm, wiiuse_motion_plus_handshake, wm->motion_plus_id, WM_EXP_ID, 6); + } + else + { + 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); + + if(val == EXP_ID_CODE_MOTION_PLUS || + val == EXP_ID_CODE_MOTION_PLUS_NUNCHUK || + val == EXP_ID_CODE_MOTION_PLUS_CLASSIC) + { + /* handshake done */ + wm->event = WIIUSE_MOTION_PLUS_ACTIVATED; + + switch(val) + { + case EXP_ID_CODE_MOTION_PLUS: + wm->exp.type = EXP_MOTION_PLUS; + break; + + case EXP_ID_CODE_MOTION_PLUS_NUNCHUK: + wm->exp.type = EXP_MOTION_PLUS_NUNCHUK; + break; + + case EXP_ID_CODE_MOTION_PLUS_CLASSIC: + wm->exp.type = EXP_MOTION_PLUS_CLASSIC; + break; + + default: + /* huh? */ + WIIUSE_WARNING("Unknown ID returned in Motion+ handshake %d\n", val); + wm->exp.type = EXP_MOTION_PLUS; + break; + } + + WIIUSE_DEBUG("Motion plus connected"); + + /* Init gyroscopes */ + 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); + } + } +} + +static void wiiuse_set_motion_plus_clear2(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); + wiiuse_status(wm); +} + +static void wiiuse_set_motion_plus_clear1(struct wiimote_t *wm,byte *data,unsigned short len) +{ + byte val = 0x00; + wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE1, &val, 1, wiiuse_set_motion_plus_clear2); +} + +/** + * @brief Enable/disable Motion+ expansion + * + * @param wm Pointer to the wiimote with Motion+ + * @param status 0 - off, 1 - on, standalone, 2 - nunchuk pass-through + * + */ +void wiiuse_set_motion_plus(struct wiimote_t *wm, int status) +{ + byte val; + + if(status && !WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP_HANDSHAKE)) + { + 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_handshake); + } + else + { + disable_expansion(wm); + val = 0x55; + wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE1, &val, 1, wiiuse_set_motion_plus_clear1); + } +} + +void motion_plus_disconnected(struct motion_plus_t* mp) +{ + WIIUSE_DEBUG("Motion plus disconnected"); + memset(mp, 0, sizeof(struct motion_plus_t)); +} + +void motion_plus_event(struct motion_plus_t* mp, int exp_type, byte* msg) +{ + /* + * Pass-through modes interleave data from the gyro + * with the expansion data. This extracts the tag + * determining which is which + */ + int isMPFrame = (1 << 1) & msg[5]; + mp->ext = msg[4] & 0x1; /* extension attached to pass-through port? */ + + if (mp->ext == 0 || isMPFrame) /* reading gyro frame */ + { + /* Check if the gyroscope is in fast or slow mode (0 if rotating fast, 1 if slow or still) */ + mp->acc_mode = ((msg[4] & 0x2) << 1) | ((msg[3] & 0x1) << 1) | ((msg[3] & 0x2) >> 1); + + mp->raw_gyro.roll = ((msg[4] & 0xFC) << 6) | msg[1]; + mp->raw_gyro.pitch = ((msg[5] & 0xFC) << 6) | msg[2]; + mp->raw_gyro.yaw = ((msg[3] & 0xFC) << 6) | msg[0]; + + /* First calibration */ + if ((mp->raw_gyro.roll > 5000) && + (mp->raw_gyro.pitch > 5000) && + (mp->raw_gyro.yaw > 5000) && + (mp->raw_gyro.roll < 0x3fff) && + (mp->raw_gyro.pitch < 0x3fff) && + (mp->raw_gyro.yaw < 0x3fff) && + !(mp->cal_gyro.roll) && + !(mp->cal_gyro.pitch) && + !(mp->cal_gyro.yaw)) + { + wiiuse_calibrate_motion_plus(mp); + } + + /* Calculate angular rates in deg/sec and performs some simple filtering */ + calculate_gyro_rates(mp); + } + + else + { + /* expansion frame */ + if (exp_type == EXP_MOTION_PLUS_NUNCHUK) + { + /* ok, this is nunchuck, re-encode it as regular nunchuck packet */ + + /* get button states */ + nunchuk_pressed_buttons(mp->nc, (msg[5] >> 2)); + + /* calculate joystick state */ + calc_joystick_state(&(mp->nc->js), msg[0], msg[1]); + + /* calculate orientation */ + mp->nc->accel.x = msg[2]; + mp->nc->accel.y = msg[3]; + mp->nc->accel.z = (msg[4] & 0xFE) | ((msg[5] >> 5) & 0x04); + + calculate_orientation(&(mp->nc->accel_calib), + &(mp->nc->accel), + &(mp->nc->orient), + NUNCHUK_IS_FLAG_SET(mp->nc, WIIUSE_SMOOTHING)); + + calculate_gforce(&(mp->nc->accel_calib), + &(mp->nc->accel), + &(mp->nc->gforce)); + + } + + else if (exp_type == EXP_MOTION_PLUS_CLASSIC) + { + WIIUSE_ERROR("Classic controller pass-through is not implemented!\n"); + } + + else + { + WIIUSE_ERROR("Unsupported mode passed to motion_plus_event() !\n"); + } + } +} + +/** + * @brief Calibrate the Motion Plus gyroscopes. + * + * @param mp Pointer to a motion_plus_t structure. + * + * This should be called only after receiving the first values + * from the Motion Plus. + */ +void wiiuse_calibrate_motion_plus(struct motion_plus_t *mp) +{ + mp->cal_gyro.roll = mp->raw_gyro.roll; + mp->cal_gyro.pitch = mp->raw_gyro.pitch; + mp->cal_gyro.yaw = mp->raw_gyro.yaw; + mp->orient.roll = 0.0; + mp->orient.pitch = 0.0; + mp->orient.yaw = 0.0; +} + +static void calculate_gyro_rates(struct motion_plus_t* mp) +{ + short int tmp_r, tmp_p, tmp_y; + float tmp_roll, tmp_pitch, tmp_yaw; + + /* We consider calibration data */ + tmp_r = mp->raw_gyro.roll - mp->cal_gyro.roll; + tmp_p = mp->raw_gyro.pitch - mp->cal_gyro.pitch; + tmp_y = mp->raw_gyro.yaw - mp->cal_gyro.yaw; + + /* We convert to degree/sec according to fast/slow mode */ + if (mp->acc_mode & 0x04) + tmp_roll = tmp_r / 20.0; + else + tmp_roll = tmp_r / 4.0; + + if (mp->acc_mode & 0x02) + tmp_pitch = tmp_p / 20.0; + else + tmp_pitch = tmp_p / 4.0; + + if (mp->acc_mode & 0x01) + tmp_yaw = tmp_y / 20.0; + else + tmp_yaw = tmp_y / 4.0; + + /* Simple filtering */ + if (fabs(tmp_roll) < 0.5) + tmp_roll = 0.0; + if (fabs(tmp_pitch) < 0.5) + tmp_pitch = 0.0; + if (fabs(tmp_yaw) < 0.5) + tmp_yaw = 0.0; + + mp->angle_rate_gyro.roll = tmp_roll; + mp->angle_rate_gyro.pitch = tmp_pitch; + mp->angle_rate_gyro.yaw = tmp_yaw; +} diff --git a/src/motion_plus.h b/src/motion_plus.h new file mode 100644 index 0000000..88cdba7 --- /dev/null +++ b/src/motion_plus.h @@ -0,0 +1,58 @@ +/* + * wiiuse + * + * Written By: + * Michal Wiedenbauer < shagkur > + * Dave Murphy < WinterMute > + * Hector Martin < marcan > + * Radu Andries + * + * Copyright 2009 + * + * This file is part of wiiuse and fWIIne. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * $Header$ + * + */ + /** + * @file + * @brief Motion plus extension + */ + +#ifndef MOTION_PLUS_H_INCLUDED +#define MOTION_PLUS_H_INCLUDED + +#include "wiiuse_internal.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** @defgroup internal_mp Internal: MotionPlus */ +/** @{ */ +void motion_plus_disconnected(struct motion_plus_t* mp); + +void motion_plus_event(struct motion_plus_t* mp, int exp_type, byte* msg); + +void wiiuse_motion_plus_handshake(struct wiimote_t *wm, byte *data,unsigned short len); + +/** @} */ + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/nunchuk.c b/src/nunchuk.c index 1e49ccb..6e9811d 100644 --- a/src/nunchuk.c +++ b/src/nunchuk.c @@ -39,9 +39,6 @@ #include /* for malloc */ #include /* for memset */ - -static void nunchuk_pressed_buttons(struct nunchuk_t* nc, byte now); - /** * @brief Handle the handshake data from the nunchuk. * @@ -164,7 +161,7 @@ void nunchuk_event(struct nunchuk_t* nc, byte* msg) { * @param nc Pointer to a nunchuk_t structure. * @param msg The message byte specified in the event packet. */ -static void nunchuk_pressed_buttons(struct nunchuk_t* nc, byte now) { +void nunchuk_pressed_buttons(struct nunchuk_t* nc, byte now) { /* message is inverted (0 is active, 1 is inactive) */ now = ~now & NUNCHUK_BUTTON_ALL; diff --git a/src/nunchuk.h b/src/nunchuk.h index 4be7da8..951749d 100644 --- a/src/nunchuk.h +++ b/src/nunchuk.h @@ -48,6 +48,8 @@ int nunchuk_handshake(struct wiimote_t* wm, struct nunchuk_t* nc, byte* data, un void nunchuk_disconnected(struct nunchuk_t* nc); void nunchuk_event(struct nunchuk_t* nc, byte* msg); + +void nunchuk_pressed_buttons(struct nunchuk_t* nc, byte now); /** @} */ #ifdef __cplusplus diff --git a/src/wiiuse.c b/src/wiiuse.c index 67e9a62..0711cad 100644 --- a/src/wiiuse.c +++ b/src/wiiuse.c @@ -147,6 +147,7 @@ struct wiimote_t** wiiuse_init(int wiimotes) { wm[i]->event = WIIUSE_NONE; wm[i]->exp.type = EXP_NONE; + wm[i]->expansion_state = 0; wiiuse_set_aspect_ratio(wm[i], WIIUSE_ASPECT_4_3); wiiuse_set_ir_position(wm[i], WIIUSE_IR_ABOVE); @@ -567,6 +568,83 @@ int wiiuse_write_data(struct wiimote_t* wm, unsigned int addr, const byte* data, return 1; } +/** + * @brief Write data to the wiimote (callback version). + * + * @param wm Pointer to a wiimote_t structure. + * @param addr The address to write to. + * @param data The data to be written to the memory location. + * @param len The length of the block to be written. + * @param cb Function pointer to call when the data arrives from the wiimote. + * + * The library can only handle one data read request at a time + * because it must keep track of the buffer and other + * events that are specific to that request. So if a request + * has already been made, subsequent requests will be added + * to a pending list and be sent out when the previous + * finishes. + */ +int wiiuse_write_data_cb(struct wiimote_t *wm, unsigned int addr, byte *data, byte len, wiiuse_write_cb write_cb) +{ + struct data_req_t* req; + + if(!wm || !WIIMOTE_IS_CONNECTED(wm)) return 0; + if( !data || !len ) return 0; + + req = (struct data_req_t*)malloc(sizeof(struct data_req_t)); + req->cb = write_cb; + req->len = len; + memcpy(req->data,data,req->len); + req->state = REQ_READY; + req->addr = addr;/* BIG_ENDIAN_LONG(addr); */ + req->next = NULL; + /* add this to the request list */ + if (!wm->data_req) { + /* root node */ + wm->data_req = req; + + WIIUSE_DEBUG("Data write request can be sent out immediately."); + + /* send the request out immediately */ + wiiuse_send_next_pending_write_request(wm); + } else { + struct data_req_t* nptr = wm->data_req; +WIIUSE_DEBUG("chaud2fois"); + for (; nptr->next; nptr = nptr->next); + nptr->next = req; + + WIIUSE_DEBUG("Added pending data write request."); + } + + return 1; +} + +/** + * @brief Send the next pending data write request to the wiimote. + * + * @param wm Pointer to a wiimote_t structure. + * + * @see wiiuse_write_data() + * + * This function is not part of the wiiuse API. + */ +void wiiuse_send_next_pending_write_request(struct wiimote_t* wm) { + struct data_req_t* req; + + if (!wm || !WIIMOTE_IS_CONNECTED(wm)) + return; + req = wm->data_req; + if (!req) + return; + if (!req->data || !req->len) + return; + if(req->state!=REQ_READY) return; + + wiiuse_write_data(wm, req->addr, req->data, req->len); + + req->state = REQ_SENT; + return; +} /** * @brief Send a packet to the wiimote. diff --git a/src/wiiuse.h b/src/wiiuse.h index 2a7d500..ce11f88 100644 --- a/src/wiiuse.h +++ b/src/wiiuse.h @@ -197,6 +197,9 @@ #define EXP_CLASSIC 2 #define EXP_GUITAR_HERO_3 3 #define EXP_WII_BOARD 4 +#define EXP_MOTION_PLUS 5 +#define EXP_MOTION_PLUS_NUNCHUK 6 /* Motion+ in nunchuk pass-through mode */ +#define EXP_MOTION_PLUS_CLASSIC 7 /* Motion+ in classic ctr. pass-through mode */ /** @} */ /** @brief IR correction types */ @@ -321,6 +324,21 @@ struct read_req_t { struct read_req_t* next; /**< next read request in the queue */ }; +/** + * @struct ang3s_t + * @brief Roll/Pitch/Yaw short angles. + */ +typedef struct ang3s_t { + int16_t roll, pitch, yaw; +} ang3s_t; + +/** + * @struct ang3f_t + * @brief Roll/Pitch/Yaw float angles. + */ +typedef struct ang3f_t { + float roll, pitch, yaw; +} ang3f_t; /** * @brief Unsigned x,y byte vector. @@ -510,6 +528,25 @@ typedef struct guitar_hero_3_t { struct joystick_t js; /**< joystick calibration */ } guitar_hero_3_t; + +/** + * @brief Motion Plus expansion device + */ +typedef struct motion_plus_t +{ + byte ext; /**< is there a device on the pass-through port? */ + + struct ang3s_t raw_gyro; /**< current raw gyroscope data */ + struct ang3s_t cal_gyro; /**< calibration raw gyroscope data */ + struct ang3f_t angle_rate_gyro; /**< current gyro angle rate */ + struct orient_t orient; /**< current orientation on each axis using Motion Plus gyroscopes */ + byte acc_mode; /**< Fast/slow rotation mode for roll, pitch and yaw (0 if rotating fast, 1 if slow or still) */ + int raw_gyro_threshold; /**< threshold for gyroscopes to generate an event */ + + struct nunchuk_t *nc; /**< pointers to nunchuk & classic in pass-through-mode */ + struct classic_ctrl_t *classic; +} motion_plus_t; + /** * @brief Wii Balance Board "expansion" device. * @@ -556,6 +593,8 @@ typedef struct wii_board_t { typedef struct expansion_t { int type; /**< type of expansion attached */ + struct motion_plus_t mp; + union { struct nunchuk_t nunchuk; struct classic_ctrl_t classic; @@ -590,6 +629,11 @@ typedef struct wiimote_state_t { float exp_r_shoulder; float exp_l_shoulder; + /* motion plus */ + short drx; + short dry; + short drz; + /* wiiboard */ uint16_t exp_wb_rtr; uint16_t exp_wb_rtl; @@ -619,6 +663,7 @@ typedef enum WIIUSE_EVENT_TYPE { WIIUSE_DISCONNECT, WIIUSE_UNEXPECTED_DISCONNECT, WIIUSE_READ_DATA, + WIIUSE_WRITE_DATA, WIIUSE_NUNCHUK_INSERTED, WIIUSE_NUNCHUK_REMOVED, WIIUSE_CLASSIC_CTRL_INSERTED, @@ -626,7 +671,9 @@ typedef enum WIIUSE_EVENT_TYPE { WIIUSE_GUITAR_HERO_3_CTRL_INSERTED, WIIUSE_GUITAR_HERO_3_CTRL_REMOVED, WIIUSE_WII_BOARD_CTRL_INSERTED, - WIIUSE_WII_BOARD_CTRL_REMOVED + WIIUSE_WII_BOARD_CTRL_REMOVED, + WIIUSE_MOTION_PLUS_ACTIVATED, + WIIUSE_MOTION_PLUS_REMOVED } WIIUSE_EVENT_TYPE; /** @@ -666,6 +713,8 @@ typedef struct wiimote_t { WCONST int flags; /**< options flag */ WCONST byte handshake_state; /**< the state of the connection handshake */ + WCONST byte expansion_state; /**< the state of the expansion handshake */ + WCONST struct data_req_t* data_req; /**< list of data read requests */ WCONST struct read_req_t* read_req; /**< list of data read requests */ WCONST struct accel_t accel_calib; /**< wiimote accelerometer calibration */ @@ -688,6 +737,7 @@ typedef struct wiimote_t { WCONST WIIUSE_EVENT_TYPE event; /**< type of event that occured */ WCONST byte event_buf[MAX_PAYLOAD]; /**< event buffer */ + WCONST byte motion_plus_id[6]; } wiimote; /** @brief Data passed to a callback during wiiuse_update() */ @@ -710,6 +760,42 @@ typedef struct wiimote_callback_data_t { /** @brief Callback type */ typedef void (*wiiuse_update_cb)(struct wiimote_callback_data_t* wm); +/** + * @brief Callback that handles a write event. + * + * @param wm Pointer to a wiimote_t structure. + * @param data Pointer to the sent data block. + * @param len Length in bytes of the data block. + * + * @see wiiuse_init() + * + * A registered function of this type is called automatically by the wiiuse + * library when the wiimote has returned the full data requested by a previous + * call to wiiuse_write_data(). + */ +typedef void (*wiiuse_write_cb)(struct wiimote_t* wm, unsigned char* data, unsigned short len); + +typedef enum data_req_s +{ + REQ_READY = 0, + REQ_SENT, + REQ_DONE +} data_req_s; + +/** + * @struct data_req_t + * @brief Data write request structure. + */ +struct data_req_t { + + byte data[21]; /**< buffer where read data is written */ + byte len; + unsigned int addr; + data_req_s state; /**< set to 1 if not using callback and needs to be cleaned up */ + wiiuse_write_cb cb; /**< read data callback */ + struct data_req_t *next; +}; + /** * @brief Loglevels supported by wiiuse. */ @@ -811,6 +897,8 @@ WIIUSE_EXPORT extern void wiiuse_set_nunchuk_accel_threshold(struct wiimote_t* w /* this function not currently implemented... */ WIIUSE_EXPORT extern void wiiuse_set_wii_board_calib(struct wiimote_t *wm); +WIIUSE_EXPORT extern void wiiuse_set_motion_plus(struct wiimote_t *wm, int status); + #ifdef __cplusplus } #endif diff --git a/src/wiiuse_internal.h b/src/wiiuse_internal.h index 339f6e6..f244b8b 100644 --- a/src/wiiuse_internal.h +++ b/src/wiiuse_internal.h @@ -129,10 +129,14 @@ /* offsets in wiimote memory */ #define WM_MEM_OFFSET_CALIBRATION 0x16 #define WM_EXP_MEM_BASE 0x04A40000 +#define WM_EXP_ID 0x04A400FA #define WM_EXP_MEM_ENABLE 0x04A40040 +#define WM_EXP_MEM_ENABLE1 0x04A400F0 +#define WM_EXP_MEM_ENABLE2 0x04A400FB #define WM_EXP_MEM_CALIBR 0x04A40020 - -#define WM_REG_IR 0x04B00030 +#define WM_EXP_MOTION_PLUS_ENABLE 0x04A600FE +#define WM_EXP_MOTION_PLUS_INIT 0x04A600F0 +#define WM_REG_IR 0x04B00030 #define WM_REG_IR_BLOCK1 0x04B00000 #define WM_REG_IR_BLOCK2 0x04B0001A #define WM_REG_IR_MODENUM 0x04B00033 @@ -162,11 +166,14 @@ * Expansion stuff */ -/* 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 +/* 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 */ #define EXP_HANDSHAKE_LEN 224 @@ -191,6 +198,11 @@ #define WIIMOTE_STATE_IR_SENS_LVL3 0x0800 #define WIIMOTE_STATE_IR_SENS_LVL4 0x1000 #define WIIMOTE_STATE_IR_SENS_LVL5 0x2000 +#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_INIT_STATES (WIIMOTE_STATE_IR_SENS_LVL3) @@ -254,8 +266,10 @@ void wiiuse_millisleep(int durationMilliseconds); int wiiuse_set_report_type(struct wiimote_t* wm); void wiiuse_send_next_pending_read_request(struct wiimote_t* wm); +void wiiuse_send_next_pending_write_request(struct wiimote_t* wm); int wiiuse_send(struct wiimote_t* wm, byte report_type, byte* msg, int len); int wiiuse_read_data_cb(struct wiimote_t* wm, wiiuse_read_cb read_cb, byte* buffer, unsigned int offset, uint16_t len); +int wiiuse_write_data_cb(struct wiimote_t *wm, unsigned int addr, byte* data, byte len, wiiuse_write_cb write_cb); #ifdef WIIUSE_DOXYGEN_PARSING