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

Merge pull request #7 from janoc/janoc-mplus-rebased

Browse Source 
Janoc mplus rebased
This commit is contained in:
Ryan Pavlik
2011-09-27 15:24:41 -07:00
parent 7dec48c44f 97768bf63c
commit bbc706b5f7
11 changed files with 505 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
CHANGELOG.mkd
View File

@@ -18,13 +18,14 @@ Added:
Added to more easily support a ctypes python binding, generated
by and included with RPythonic http://code.google.com/p/rpythonic/
Thanks to hartsantler for this feature.
- Ability to build as a static library - must define WIIUSE_STATIC in
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, merged from fwiine - thanks to admiral0 for
the pull request.
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:
@@ -33,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]

13
README.mkd
View File

@@ -52,9 +52,10 @@ Additional Contributors:
- Paul Burton <https://github.com/paulburton/wiiuse>
- Karl Semich <https://github.com/xloem>
- Johannes Zarl <johannes.zarl@jku.at>
- hartsantler <http://code.google.com/p/rpythonic/>
- admiral0 and fwiine project <http://sourceforge.net/projects/fwiine/files/wiiuse/0.13/>
- Jeff Baker/Inv3rsion, LLC. <http://www.inv3rsion.com/>
- hartsantler <http://code.google.com/p/rpythonic/>
- admiral0 and fwiine project <http://sourceforge.net/projects/fwiine/files/wiiuse/0.13/>
- Jeff Baker/Inv3rsion, LLC. <http://www.inv3rsion.com/>
- Jan Ciger - Reviatech SAS <jan.ciger@reviatech.com>
License
@@ -171,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
------------------------------------
<http://wiibrew.org/>

268
src/events.c
View File

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

34
src/io.c
View File

@@ -31,10 +31,28 @@
* @brief Handles device I/O (non-OS specific).
*/
#include "io.h"
#include "ir.h" /* for wiiuse_set_ir_mode */
#include <stdlib.h> /* 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;

216
src/motion_plus.c
View File

@@ -32,37 +32,84 @@
#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 <string.h> /* for memset */
#include <math.h> /* for fabs */
void wiiuse_motion_plus_check(struct wiimote_t *wm,byte *data,unsigned short len)
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_check, wm->motion_plus_id, WM_EXP_ID, 6);
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);
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)
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;
wm->exp.type = EXP_MOTION_PLUS;
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");
WIIMOTE_ENABLE_STATE(wm,WIIMOTE_STATE_EXP);
/* 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);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_FAILED);
WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
wiiuse_set_ir_mode(wm);
@@ -75,18 +122,22 @@ static void wiiuse_set_motion_plus_clear1(struct wiimote_t *wm,byte *data,unsign
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(WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP_HANDSHAKE))
return;
WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE);
if(status)
if(status && !WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP_HANDSHAKE))
{
val = 0x04;
wiiuse_write_data_cb(wm, WM_EXP_MOTION_PLUS_ENABLE, &val, 1, wiiuse_motion_plus_check);
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
{
@@ -102,12 +153,137 @@ void motion_plus_disconnected(struct motion_plus_t* mp)
memset(mp, 0, sizeof(struct motion_plus_t));
}
void motion_plus_event(struct motion_plus_t* mp, byte* msg)
void motion_plus_event(struct motion_plus_t* mp, int exp_type, byte* msg)
{
mp->rx = ((msg[5] & 0xFC) << 6) | msg[2]; /* Pitch */
mp->ry = ((msg[4] & 0xFC) << 6) | msg[1]; /* Roll */
mp->rz = ((msg[3] & 0xFC) << 6) | msg[0]; /* Yaw */
/*
* 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? */
mp->ext = msg[4] & 0x1;
mp->status = (msg[3] & 0x3) | ((msg[4] & 0x2) << 1); /* roll, yaw, pitch */
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;
}

5
src/motion_plus.h
View File

@@ -45,7 +45,10 @@ extern "C" {
/** @{ */
void motion_plus_disconnected(struct motion_plus_t* mp);
void motion_plus_event(struct motion_plus_t* mp, byte* msg);
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

5
src/nunchuk.c
View File

@@ -39,9 +39,6 @@
#include <stdlib.h> /* for malloc */
#include <string.h> /* 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;

2
src/nunchuk.h
View File

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

1
src/wiiuse.c
View File

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

37
src/wiiuse.h
View File

@@ -198,6 +198,8 @@
#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 */
@@ -322,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.
@@ -517,9 +534,17 @@ typedef struct guitar_hero_3_t {
*/
typedef struct motion_plus_t
{
short rx, ry, rz;
unsigned char status;
unsigned char ext;
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;
/**
@@ -568,12 +593,13 @@ 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;
struct guitar_hero_3_t gh3;
struct wii_board_t wb;
struct motion_plus_t mp;
};
} expansion_t;
@@ -637,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,
@@ -686,7 +713,7 @@ typedef struct wiimote_t {
WCONST int flags; /**< options flag */
WCONST byte handshake_state; /**< the state of the connection handshake */
WCONST unsigned char expansion_state; /**< the state of the expansion 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 */

24
src/wiiuse_internal.h
View File

@@ -135,6 +135,7 @@
#define WM_EXP_MEM_ENABLE2 0x04A400FB
#define WM_EXP_MEM_CALIBR 0x04A40020
#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
@@ -165,12 +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
#define EXP_ID_CODE_MOTION_PLUS 0xa4200405
/* 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
@@ -184,8 +187,6 @@
#define WIIMOTE_STATE_DEV_FOUND 0x0001
#define WIIMOTE_STATE_HANDSHAKE 0x0002 /* actual connection exists but no handshake yet */
#define WIIMOTE_STATE_HANDSHAKE_COMPLETE 0x0004 /* actual connection exists but no handshake yet */
#define WIIMOTE_STATE_EXP_HANDSHAKE 0x00020 /* actual connection exists but no handshake yet */
#define WIIMOTE_STATE_EXP_FAILED 0x00040 /* actual connection exists but no handshake yet */
#define WIIMOTE_STATE_CONNECTED 0x0008
#define WIIMOTE_STATE_RUMBLE 0x0010
#define WIIMOTE_STATE_ACC 0x0020
@@ -197,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)
@@ -263,7 +269,7 @@ 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, uint addr, unsigned char *data, unsigned char len, wiiuse_write_cb write_cb);
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