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

Merged stuff from fWiine, Wiiuse master and local Motion+ modifs, not working yet

Browse Source 
Dos2unix and unexpanded, then selectively committed by rpavlik
This commit is contained in:
Jan Ciger
2011-09-14 08:51:23 -05:00
committed by Ryan Pavlik
parent 7dec48c44f
commit 541cbb5156
7 changed files with 338 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

140
src/events.c
View File

@@ -60,6 +60,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 +397,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 +547,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 +606,7 @@ 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;
/*
* An event occured.
@@ -613,13 +661,33 @@ 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)) {
/*
* 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);
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
wiiuse_set_report_type(wm);
}
@@ -822,6 +890,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.p;
wm->lstate.dry = wm->exp.mp.raw_gyro.r;
wm->lstate.drz = wm->exp.mp.raw_gyro.y;
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 +1020,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.p);
STATE_CHANGED(wm->lstate.dry, wm->exp.mp.raw_gyro.r);
STATE_CHANGED(wm->lstate.drz, wm->exp.mp.raw_gyro.y);
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;

3
src/io.c
View File

@@ -105,6 +105,9 @@ 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:

173
src/motion_plus.c
View File

@@ -32,15 +32,21 @@
#include "events.h" /* for disable_expansion */
#include "ir.h" /* for wiiuse_set_ir_mode */
#include "nunchuk.h"
#include "dynamics.h"
#include <string.h> /* for memset */
#include <math.h> /* 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_check(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_check, wm->motion_plus_id, WM_EXP_ID, 6);
}
else
{
@@ -48,13 +54,50 @@ void wiiuse_motion_plus_check(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);
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.r = 0;
wm->exp.mp.cal_gyro.p = 0;
wm->exp.mp.cal_gyro.y = 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;
wiiuse_set_ir_mode(wm);
}
}
@@ -104,10 +147,124 @@ void motion_plus_disconnected(struct motion_plus_t* mp)
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 */
/*
* 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.r = ((msg[4] & 0xFC) << 6) | msg[1];
mp->raw_gyro.p = ((msg[5] & 0xFC) << 6) | msg[2];
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))
{
wiiuse_calibrate_motion_plus(mp);
}
/* Calculate angular rates in deg/sec and performs some simple filtering */
calculate_gyro_rates(mp);
}
else
{ /* expansion frame */
/* FIXME: Handle pass-through modes */
/* if (mp->mode == WIIUSE_MP_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 (mp->mode == WIIUSE_MP_CLASSIC) */
/* { */
/* WIIUSE_ERROR("Classic controller pass-through is not implemented!\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.r = mp->raw_gyro.r;
mp->cal_gyro.p = mp->raw_gyro.p;
mp->cal_gyro.y = mp->raw_gyro.y;
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.r - mp->cal_gyro.r;
tmp_p = mp->raw_gyro.p - mp->cal_gyro.p;
tmp_y = mp->raw_gyro.y - mp->cal_gyro.y;
/* 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.r = tmp_roll;
mp->angle_rate_gyro.p = tmp_pitch;
mp->angle_rate_gyro.y = tmp_yaw;
}

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

33
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 RPY short angles.
*/
typedef struct ang3s_t {
short r, p, y;
} ang3s_t;
/**
* @struct ang3f_t
* @brief RPY float angles.
*/
typedef struct ang3f_t {
float r, p, y;
} 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;
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,

7
src/wiiuse_internal.h
View File

@@ -171,6 +171,8 @@
#define EXP_ID_CODE_CLASSIC_CONTROLLER 0x9A1EFDFD
#define EXP_ID_CODE_GUITAR 0x9A1EFDFB
#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 +186,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 +197,9 @@
#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 0x40000 /* actual M+ connection exists but no handshake yet */
#define WIIMOTE_STATE_EXP_FAILED 0x80000 /* actual M+ connection exists but handshake failed */
#define WIIMOTE_INIT_STATES (WIIMOTE_STATE_IR_SENS_LVL3)