diff --git a/src/events.c b/src/events.c index 5e1f803..4ef796a 100644 --- a/src/events.c +++ b/src/events.c @@ -164,6 +164,9 @@ int wiiuse_poll(struct wiimote_t** wm, int wiimotes) { propagate_event(wm[i], wm[i]->event_buf[1], wm[i]->event_buf+2); evnt += (wm[i]->event != WIIUSE_NONE); } else { + /* send out any waiting writes */ + wiiuse_send_next_pending_write_request(wm[i]); + idle_cycle(wm[i]); } } @@ -186,6 +189,9 @@ int wiiuse_poll(struct wiimote_t** wm, int wiimotes) { /* clear out the event buffer */ memset(wm[i]->event_buf, 0, sizeof(wm[i]->event_buf)); } else { + /* send out any waiting writes */ + wiiuse_send_next_pending_write_request(wm[i]); + idle_cycle(wm[i]); } } @@ -650,9 +656,16 @@ static void event_status(struct wiimote_t* wm, byte* msg) { if (msg[2] & WM_CTRL_STATUS_BYTE1_LED_3) led[2] = 1; if (msg[2] & WM_CTRL_STATUS_BYTE1_LED_4) led[3] = 1; + /* probe for Motion+ */ + if(!WIIMOTE_IS_SET(wm, WIIMOTE_STATE_MPLUS_PRESENT)) + wiiuse_probe_motion_plus(wm); + /* is an attachment connected to the expansion port? */ if ((msg[2] & WM_CTRL_STATUS_BYTE1_ATTACHMENT) == WM_CTRL_STATUS_BYTE1_ATTACHMENT) + { + WIIUSE_DEBUG("Attachment detected!"); attachment = 1; + } /* is the speaker enabled? */ if ((msg[2] & WM_CTRL_STATUS_BYTE1_SPEAKER_ENABLED) == WM_CTRL_STATUS_BYTE1_SPEAKER_ENABLED) @@ -708,7 +721,6 @@ static void event_status(struct wiimote_t* wm, byte* msg) { req->state = REQ_DONE; /* if(req->cb!=NULL) req->cb(wm,msg,6); */ free(req); - wiiuse_send_next_pending_write_request(wm); } diff --git a/src/io.c b/src/io.c index 168ff9f..25f62d4 100644 --- a/src/io.c +++ b/src/io.c @@ -32,9 +32,11 @@ */ #include "io.h" #include "ir.h" /* for wiiuse_set_ir_mode */ +#include "wiiuse_internal.h" #include /* for free, malloc */ +extern void propagate_event(struct wiimote_t* wm, byte event, byte* msg); static void wiiuse_disable_motion_plus2(struct wiimote_t *wm,byte *data,unsigned short len) { @@ -53,6 +55,85 @@ static void wiiuse_disable_motion_plus1(struct wiimote_t *wm,byte *data,unsigned wiiuse_write_data_cb(wm, WM_EXP_MEM_ENABLE1, &val, 1, wiiuse_disable_motion_plus2); } + /** +* @brief Wait until specified report arrives and return it +* +* @param wm Pointer to a wiimote_t structure. +* @param data Pre-allocated memory to store the received data +* +* Synchronous/blocking, this function will not return until it receives the specified +* report from the Wiimote. +* +*/ +void wiiuse_wait_report(struct wiimote_t *wm, int report, byte *data) +{ + for(;;) + { + if(wiiuse_io_read(wm, data, 32) > 0) + if(data[1] == report) + break; + } +} + +/** +* @brief Read memory/register data synchronously +* +* @param wm Pointer to a wiimote_t structure. +* @param memory If set to non-zero, reads EEPROM, otherwise registers +* @param addr Address offset to read from +* @param size How many bytes to read +* @param data Pre-allocated memory to store the received data +* +* Synchronous/blocking read, this function will not return until it receives the specified +* amount of data from the Wiimote. +* +*/ +void wiiuse_read(struct wiimote_t *wm, byte memory, unsigned addr, unsigned short size, byte *data) +{ + byte pkt[8]; + byte buf[32]; + unsigned n_full_reports; + unsigned last_report; + byte *output; + int i; + + /* + * address in big endian first, the leading byte will + * be overwritten (only 3 bytes are sent) + */ + to_big_endian_uint32_t(pkt + 2, addr); + + pkt[0] = 0x52; /* HID read command */ + pkt[1] = 0x17; /* report 17 - read */ + pkt[2] = (memory != 0) ? 0x00 : 0x04; /* read from registers or memory*/ + + /* + * length in big endian + */ + to_big_endian_uint16_t(pkt + 6, size); + + /* calculate how many 16B packets we have to get back */ + n_full_reports = size / 16; + last_report = size % 16; + output = data; + + wiiuse_io_write(wm, pkt, 8); + + for(i = 0; i < n_full_reports; ++i) + { + wiiuse_wait_report(wm, 0x21, buf); + memmove(output, buf + 7, 16); + output += 16; + } + + /* read the last incomplete packet */ + if(last_report) + { + wiiuse_wait_report(wm, 0x21, buf); + memmove(output, buf + 7, last_report); + } +} + /** * @brief Get initialization data from the wiimote. * @@ -66,18 +147,93 @@ static void wiiuse_disable_motion_plus1(struct wiimote_t *wm,byte *data,unsigned * The handshake will be concluded when the wiimote responds * with this data. */ + +#define SYNC_HANDSHAKE 1 +#ifdef SYNC_HANDSHAKE + +void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) +{ + /* send request to wiimote for accelerometer calibration */ + byte buf[32]; + byte report_type; + + /* step 0 - Reset wiimote */ + { + //wiiuse_set_leds(wm, WIIMOTE_LED_NONE); + + WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE); + WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_CONNECTED); + WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_ACC); + WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR); + WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_RUMBLE); + WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_EXP); + WIIMOTE_DISABLE_FLAG(wm, WIIUSE_CONTINUOUS); + + wiiuse_set_report_type(wm); + wiiuse_millisleep(500); + + WIIUSE_DEBUG("Wiimote reset!\n"); + } + + /* step 1 - calibration of accelerometers */ + { + struct accel_t* accel = &wm->accel_calib; + byte val; + + wiiuse_read(wm, 1, WM_MEM_OFFSET_CALIBRATION, 8, buf); + + /* received read data */ + accel->cal_zero.x = buf[0]; + accel->cal_zero.y = buf[1]; + accel->cal_zero.z = buf[2]; + + accel->cal_g.x = buf[4] - accel->cal_zero.x; + accel->cal_g.y = buf[5] - accel->cal_zero.y; + accel->cal_g.z = buf[6] - accel->cal_zero.z; + + WIIUSE_DEBUG("Calibrated wiimote acc\n"); + } + + /* step 2 - re-enable IR and ask for status */ + { + WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE_COMPLETE); + WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE); + + /* now enable IR if it was set before the handshake completed */ + if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_IR)) + { + WIIUSE_DEBUG("Handshake finished, enabling IR."); + WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR); + wiiuse_set_ir(wm, 1); + } + + WIIUSE_DEBUG("Asking for status ...\n"); + wm->event = WIIUSE_CONNECT; + wiiuse_status(wm); + } +} + +#else void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) { if (!wm) return; switch (wm->handshake_state) { case 0: { - /* send request to wiimote for accelerometer calibration */ byte* buf; + /* continous reporting off, report to buttons only */ WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_HANDSHAKE); wiiuse_set_leds(wm, WIIMOTE_LED_NONE); + WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_ACC); + WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_IR); + WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_RUMBLE); + WIIMOTE_DISABLE_FLAG(wm, WIIUSE_CONTINUOUS); + + wiiuse_set_report_type(wm); + + /* send request to wiimote for accelerometer calibration */ buf = (byte*)malloc(sizeof(byte) * 8); wiiuse_read_data_cb(wm, wiiuse_handshake, buf, WM_MEM_OFFSET_CALIBRATION, 7); wm->handshake_state++; @@ -143,3 +299,4 @@ void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len) { } } } +#endif diff --git a/src/io.h b/src/io.h index 246774f..20da950 100644 --- a/src/io.h +++ b/src/io.h @@ -52,7 +52,10 @@ void wiiuse_handshake(struct wiimote_t* wm, byte* data, uint16_t len); void wiiuse_init_platform_fields(struct wiimote_t* wm); void wiiuse_cleanup_platform_fields(struct wiimote_t* wm); -int wiiuse_io_read(struct wiimote_t* wm); +void wiiuse_wait_report(struct wiimote_t *wm, int report, byte *data); +void wiiuse_read(struct wiimote_t *wm, byte memory, unsigned addr, unsigned short size, byte *data); + +int wiiuse_io_read(struct wiimote_t* wm, byte* buf, int len); int wiiuse_io_write(struct wiimote_t* wm, byte* buf, int len); /** @} */ diff --git a/src/io_nix.c b/src/io_nix.c index fb14735..703e065 100644 --- a/src/io_nix.c +++ b/src/io_nix.c @@ -267,14 +267,26 @@ void wiiuse_disconnect(struct wiimote_t* wm) { } -int wiiuse_io_read(struct wiimote_t* wm) { - /* not used */ - return 0; +int wiiuse_io_read(struct wiimote_t* wm, byte* buf, int len) { + int rc; + + rc = read(wm->in_sock, buf, len); + + if(rc <= 0) + wiiuse_disconnected(wm); + + return rc; } int wiiuse_io_write(struct wiimote_t* wm, byte* buf, int len) { - return write(wm->out_sock, buf, len); + int rc; + rc = write(wm->out_sock, buf, len); + + if(rc < 0) + wiiuse_disconnected(wm); + + return rc; } void wiiuse_init_platform_fields(struct wiimote_t* wm) { diff --git a/src/motion_plus.c b/src/motion_plus.c index 27d0d0e..c69f3b2 100644 --- a/src/motion_plus.c +++ b/src/motion_plus.c @@ -30,6 +30,7 @@ #include "motion_plus.h" +#include "wiiuse_internal.h" /* for endian conversions */ #include "events.h" /* for disable_expansion */ #include "ir.h" /* for wiiuse_set_ir_mode */ #include "nunchuk.h" /* for nunchuk_pressed_buttons */ @@ -42,6 +43,68 @@ static void wiiuse_calibrate_motion_plus(struct motion_plus_t *mp); static void calculate_gyro_rates(struct motion_plus_t* mp); +void wiiuse_probe_motion_plus(struct wiimote_t *wm) +{ + byte buf[32]; + unsigned id; + + wiiuse_read(wm, 0, WM_EXP_MOTION_PLUS_IDENT, 6, buf); + + /* check error code */ + if(buf[4] & 0x0f) + { + WIIUSE_DEBUG("No Motion+ available, stopping probe."); + WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_MPLUS_PRESENT); + return; + } + + /* decode the id*/ + id = from_big_endian_uint32_t(buf + 2); + + if(id != EXP_ID_CODE_INACTIVE_MOTION_PLUS && + id != EXP_ID_CODE_NLA_MOTION_PLUS && + id != EXP_ID_CODE_NLA_MOTION_PLUS_NUNCHUK && + id != EXP_ID_CODE_NLA_MOTION_PLUS_CLASSIC) + { + /* we have read something weird */ + WIIUSE_DEBUG("Motion+ ID doesn't match, probably not connected."); + WIIMOTE_DISABLE_STATE(wm, WIIMOTE_STATE_MPLUS_PRESENT); + return; + } + + WIIUSE_DEBUG("Detected inactive Motion+!"); + WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_MPLUS_PRESENT); + + /* init M+ */ + buf[0] = 0x55; + wiiuse_write_data(wm, WM_EXP_MOTION_PLUS_INIT, buf, 1); + + /* Init whatever is hanging on the pass-through port */ + buf[0] = 0x55; + wiiuse_write_data(wm, WM_EXP_MEM_ENABLE1, buf, 1); + + buf[0] = 0x00; + wiiuse_write_data(wm, WM_EXP_MEM_ENABLE2, buf, 1); + + /* Init gyroscope data */ + 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); +} + void wiiuse_motion_plus_handshake(struct wiimote_t *wm,byte *data,unsigned short len) { uint32_t val; @@ -133,7 +196,11 @@ void wiiuse_set_motion_plus(struct wiimote_t *wm, int status) { byte val; - if(status && !WIIMOTE_IS_SET(wm,WIIMOTE_STATE_EXP_HANDSHAKE)) + if(!WIIMOTE_IS_SET(wm, WIIMOTE_STATE_MPLUS_PRESENT) || + WIIMOTE_IS_SET(wm, WIIMOTE_STATE_EXP_HANDSHAKE)) + return; + + if(status) { WIIMOTE_ENABLE_STATE(wm, WIIMOTE_STATE_EXP_HANDSHAKE); val = (status == 1) ? 0x04 : 0x05; diff --git a/src/wiiuse_internal.h b/src/wiiuse_internal.h index 6aed90c..1403d52 100644 --- a/src/wiiuse_internal.h +++ b/src/wiiuse_internal.h @@ -172,6 +172,7 @@ #define WM_EXP_MEM_ENABLE1 0x04A400F0 #define WM_EXP_MEM_ENABLE2 0x04A400FB #define WM_EXP_MEM_CALIBR 0x04A40020 +#define WM_EXP_MOTION_PLUS_IDENT 0x04A600FA #define WM_EXP_MOTION_PLUS_ENABLE 0x04A600FE #define WM_EXP_MOTION_PLUS_INIT 0x04A600F0 #define WM_REG_IR 0x04B00030 @@ -209,10 +210,16 @@ #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 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 */ +/* decrypted M+ codes at 0x04A600FA */ +#define EXP_ID_CODE_INACTIVE_MOTION_PLUS 0xA6200005 /** Inactive Motion Plus ID */ +#define EXP_ID_CODE_NLA_MOTION_PLUS 0xA6200405 /** No longer active Motion Plus ID */ +#define EXP_ID_CODE_NLA_MOTION_PLUS_NUNCHUK 0xA6200505 /** No longer active Motion Plus ID in Nunchuck passthrough mode */ +#define EXP_ID_CODE_NLA_MOTION_PLUS_CLASSIC 0xA6200705 /** No longer active Motion Plus ID in Classic control. passthrough */ + #define EXP_HANDSHAKE_LEN 224 /******************** @@ -239,6 +246,7 @@ #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_STATE_MPLUS_PRESENT 0x80000 /* Motion+ is connected */