From 1a9293ab296ce949010bbc9401e8e3d6e5a0c6b5 Mon Sep 17 00:00:00 2001 From: Radu Andries Date: Sat, 27 Aug 2011 21:46:36 +0200 Subject: [PATCH] Added initial support for Motion Plus from fork http://sourceforge.net/projects/fwiine/files/wiiuse/0.13/ --- src/CMakeLists.txt | 2 + src/ir.c | 11 ++++ src/motion_plus.c | 124 ++++++++++++++++++++++++++++++++++++++++++ src/motion_plus.h | 23 ++++++++ src/wiiuse.c | 98 +++++++++++++++++++++++++++++++++ src/wiiuse.h | 63 ++++++++++++++++++++- src/wiiuse_internal.h | 10 +++- 7 files changed, 328 insertions(+), 3 deletions(-) create mode 100644 src/motion_plus.c create mode 100644 src/motion_plus.h 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/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..2d31201 --- /dev/null +++ b/src/motion_plus.c @@ -0,0 +1,124 @@ +/* + * 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 +#include +#include +#include +#include + +#ifdef WIN32 + #include +#endif + +#include "definitions.h" +#include "wiiuse_internal.h" +#include "dynamics.h" +#include "events.h" +//#include ".h" +#include "io.h" + +void wiiuse_motion_plus_check(struct wiimote_t *wm,byte *data,unsigned short len) +{ + int val; + if(data == NULL) + { + wiiuse_read_data_cb(wm, wiiuse_motion_plus_check, 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); + val = (data[3] << 16) | (data[2] << 24) | (data[4] << 8) | data[5]; + if(val == EXP_ID_CODE_MOTION_PLUS) + { + /* handshake done */ + wm->event = WIIUSE_MOTION_PLUS_ACTIVATED; + wm->exp.type = EXP_MOTION_PLUS; + WIIUSE_DEBUG("Motion plus connected"); + WIIMOTE_ENABLE_STATE(wm,WIIMOTE_STATE_EXP); + wiiuse_set_ir_mode(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); + wiiuse_status(wm); +} + +static void wiiuse_set_motion_plus_clear1(struct wiimote_t *wm,byte *data,unsigned short len) +{ + unsigned char val = 0x00; + wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE1, &val, 1, wiiuse_set_motion_plus_clear2); +} + + +void wiiuse_set_motion_plus(struct wiimote_t *wm, int status) +{ + unsigned char val; + + if(WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP_HANDSHAKE)) + return; + + WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE); + if(status) + { + val = 0x04; + wiiuse_write_data_cb(wm, WM_EXP_MOTION_PLUS_ENABLE, &val, 1, wiiuse_motion_plus_check); + } + 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, 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 + + mp->ext = msg[4] & 0x1; + mp->status = (msg[3] & 0x3) | ((msg[4] & 0x2) << 1); // roll, yaw, pitch +} diff --git a/src/motion_plus.h b/src/motion_plus.h new file mode 100644 index 0000000..0f017e9 --- /dev/null +++ b/src/motion_plus.h @@ -0,0 +1,23 @@ +/** + * @file + * @brief Motion plus extension + */ + +#ifndef MOTION_PLUS_H_INCLUDED +#define MOTION_PLUS_H_INCLUDED + +#include "wiiuse_internal.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void motion_plus_disconnected(struct motion_plus_t* mp); + +void motion_plus_event(struct motion_plus_t* mp, byte* msg); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/wiiuse.c b/src/wiiuse.c index 67e9a62..625afea 100644 --- a/src/wiiuse.c +++ b/src/wiiuse.c @@ -567,6 +567,104 @@ 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,uint addr,unsigned char *data,unsigned char 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) { + byte buf[21] = {0}; /* the payload is always 23 */ + struct data_req_t* req = wm->data_req; + + if (!wm || !WIIMOTE_IS_CONNECTED(wm)) + return; + if (!req->data || !req->len) + return; + if(req->state!=REQ_READY) return; + req = wm->data_req; + if (!req) + return; + + WIIUSE_DEBUG("Writing %i bytes to memory location 0x%x...", req->len, req->addr); + + #ifdef WITH_WIIUSE_DEBUG + { + unsigned int i = 0; + printf("Write data is: "); + for (; i < req->len; ++i) + printf("%x ", req->data[i]); + printf("\n"); + } + #endif + + /* the offset is in big endian */ + *(int*)(buf) = BIG_ENDIAN_LONG(req->addr); + + /* length */ + *(byte*)(buf + 4) = req->len;//BIG_ENDIAN_SHORT(req->len); + + /* data */ + memcpy(buf + 5, req->data, req->len); + + wiiuse_send(wm, WM_CMD_WRITE_DATA, buf, 21); + req->state = REQ_SENT; + return; +} /** * @brief Send a packet to the wiimote. diff --git a/src/wiiuse.h b/src/wiiuse.h index 2a7d500..d681dca 100644 --- a/src/wiiuse.h +++ b/src/wiiuse.h @@ -197,6 +197,7 @@ #define EXP_CLASSIC 2 #define EXP_GUITAR_HERO_3 3 #define EXP_WII_BOARD 4 +#define EXP_MOTION_PLUS 5 /** @} */ /** @brief IR correction types */ @@ -510,6 +511,17 @@ 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 +{ + short rx, ry, rz; + unsigned char status; + unsigned char ext; +} motion_plus_t; + /** * @brief Wii Balance Board "expansion" device. * @@ -561,6 +573,7 @@ typedef struct expansion_t { struct classic_ctrl_t classic; struct guitar_hero_3_t gh3; struct wii_board_t wb; + struct motion_plus_t mp; }; } expansion_t; @@ -589,6 +602,11 @@ typedef struct wiimote_state_t { struct vec3b_t exp_accel; float exp_r_shoulder; float exp_l_shoulder; + + /* motion plus */ + short drx; + short dry; + short drz; /* wiiboard */ uint16_t exp_wb_rtr; @@ -626,7 +644,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 +686,8 @@ 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 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 +710,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 +733,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 { + + unsigned char data[21]; /**< buffer where read data is written */ + unsigned int 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 +870,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..8b43ca4 100644 --- a/src/wiiuse_internal.h +++ b/src/wiiuse_internal.h @@ -129,10 +129,13 @@ /* 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_REG_IR 0x04B00030 #define WM_REG_IR_BLOCK1 0x04B00000 #define WM_REG_IR_BLOCK2 0x04B0001A #define WM_REG_IR_MODENUM 0x04B00033 @@ -167,6 +170,7 @@ #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_HANDSHAKE_LEN 224 @@ -180,6 +184,8 @@ #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