From 541cbb5156568adbf49628d85f3acf907008cd47 Mon Sep 17 00:00:00 2001 From: Jan Ciger Date: Wed, 14 Sep 2011 08:51:23 -0500 Subject: [PATCH] Merged stuff from fWiine, Wiiuse master and local Motion+ modifs, not working yet Dos2unix and unexpanded, then selectively committed by rpavlik --- src/events.c | 140 ++++++++++++++++++++++++++++++++-- src/io.c | 3 + src/motion_plus.c | 173 ++++++++++++++++++++++++++++++++++++++++-- src/nunchuk.c | 5 +- src/nunchuk.h | 2 + src/wiiuse.h | 33 +++++++- src/wiiuse_internal.h | 7 +- 7 files changed, 338 insertions(+), 25 deletions(-) diff --git a/src/events.c b/src/events.c index 8a895ad..6fe9621 100644 --- a/src/events.c +++ b/src/events.c @@ -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; diff --git a/src/io.c b/src/io.c index 44d2c36..d3a956c 100644 --- a/src/io.c +++ b/src/io.c @@ -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: diff --git a/src/motion_plus.c b/src/motion_plus.c index 46b8b50..3371f97 100644 --- a/src/motion_plus.c +++ b/src/motion_plus.c @@ -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 /* 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_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; } 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.h b/src/wiiuse.h index fcf1336..6279a07 100644 --- a/src/wiiuse.h +++ b/src/wiiuse.h @@ -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, diff --git a/src/wiiuse_internal.h b/src/wiiuse_internal.h index b742fae..af342aa 100644 --- a/src/wiiuse_internal.h +++ b/src/wiiuse_internal.h @@ -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)