From c639a3ee4f3634b93804cbabfd759ab26c8cd567 Mon Sep 17 00:00:00 2001 From: Kristian Sloth Lauszus Date: Thu, 10 Jan 2013 20:10:16 +0100 Subject: [PATCH] Now works with the new Wiimote with Motion Plus Inside --- BTD.cpp | 14 ++++++++++++++ BTD.h | 1 + Wii.cpp | 22 +++++++++++++--------- 3 files changed, 28 insertions(+), 9 deletions(-) diff --git a/BTD.cpp b/BTD.cpp index a2c91003..35570a24 100755 --- a/BTD.cpp +++ b/BTD.cpp @@ -375,6 +375,10 @@ void BTD::HCI_event_task() { #endif for(uint8_t i = 0; i < hcibuf[2]; i++) { if((hcibuf[4+8*hcibuf[2]+3*i] == 0x04 && hcibuf[5+8*hcibuf[2]+3*i] == 0x25 && hcibuf[6+8*hcibuf[2]+3*i] == 0x00) || (hcibuf[4+8*hcibuf[2]+3*i] == 0x08 && hcibuf[5+8*hcibuf[2]+3*i] == 0x05 && hcibuf[6+8*hcibuf[2]+3*i] == 0x00)) { // See http://bluetooth-pentest.narod.ru/software/bluetooth_class_of_device-service_generator.html and http://wiibrew.org/wiki/Wiimote#SDP_information + if(hcibuf[4+8*hcibuf[2]+3*i] == 0x08) // Check if it's the new Wiimote with motion plus inside that was detected + motionPlusInside = true; + else + motionPlusInside = false; disc_bdaddr[0] = hcibuf[3+6*i]; disc_bdaddr[1] = hcibuf[4+6*i]; disc_bdaddr[2] = hcibuf[5+6*i]; @@ -591,6 +595,8 @@ void BTD::HCI_task() { hci_inquiry_cancel(); // Stop inquiry #ifdef DEBUG Notify(PSTR("\r\nWiimote found")); + if(motionPlusInside) + Notify(PSTR(" with Motion Plus Inside")); Notify(PSTR("\r\nNow just create the instance like so:")); Notify(PSTR("\r\nWII Wii(&Btd);")); Notify(PSTR("\r\nAnd then press any button on the Wiimote")); @@ -679,6 +685,14 @@ void BTD::HCI_task() { #ifdef DEBUG Notify(PSTR("\r\nWiimote is connecting")); #endif + if(strncmp((const char*)remote_name, "Nintendo RVL-CNT-01-TR", 22) == 0) { +#ifdef DEBUG + Notify(PSTR(" with Motion Plus Inside")); +#endif + motionPlusInside = true; + } else + motionPlusInside = false; + incomingWii = true; } hci_event_flag = 0; diff --git a/BTD.h b/BTD.h index 7dd4a6f6..6efa3a5a 100755 --- a/BTD.h +++ b/BTD.h @@ -186,6 +186,7 @@ public: bool connectToWii; // Used to only send the ACL data to the wiimote bool incomingWii; bool pairWithWii; + bool motionPlusInside; // True if it's the new Wiimote with the Motion Plus Inside /* HCI Commands */ void HCI_Command(uint8_t* data, uint16_t nbytes); diff --git a/Wii.cpp b/Wii.cpp index b4109624..922188fa 100755 --- a/Wii.cpp +++ b/Wii.cpp @@ -53,6 +53,7 @@ void WII::disconnect() { // Use this void to disconnect any of the controllers pBtd->l2cap_disconnection_request(hci_handle,0x0A, interrupt_scid, interrupt_dcid); Reset(); l2cap_state = L2CAP_INTERRUPT_DISCONNECT; + pBtd->motionPlusInside = false; } void WII::ACLData(uint8_t* l2capinbuf) { @@ -252,29 +253,29 @@ void WII::ACLData(uint8_t* l2capinbuf) { case 0x21: // Read Memory Data if((l2capinbuf[12] & 0x0F) == 0) { // No error // See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers - if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) { + if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) { #ifdef DEBUG Notify(PSTR("\r\nNunchuck connected")); #endif l2cap_event_flag |= WII_FLAG_NUNCHUCK_CONNECTED; - } else if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA6 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) { + } else if(l2capinbuf[16] == 0x00 && (l2capinbuf[17] == 0xA6 || l2capinbuf[17] == 0xA4) && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) { #ifdef DEBUG Notify(PSTR("\r\nMotion Plus connected")); #endif l2cap_event_flag |= WII_FLAG_MOTION_PLUS_CONNECTED; - } else if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) { + } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) { #ifdef DEBUG Notify(PSTR("\r\nMotion Plus activated in normal mode")); #endif motionPlusConnected = true; - } else if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) { + } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) { #ifdef DEBUG Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode")); #endif activateNunchuck = false; motionPlusConnected = true; nunchuckConnected = true; - } else if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA6 && l2capinbuf[18] == 0x20 && (l2capinbuf[19] == 0x00 || l2capinbuf[19] == 0x04 || l2capinbuf[19] == 0x05 || l2capinbuf[19] == 0x07) && l2capinbuf[20] == 0x05) { + } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA6 && l2capinbuf[18] == 0x20 && (l2capinbuf[19] == 0x00 || l2capinbuf[19] == 0x04 || l2capinbuf[19] == 0x05 || l2capinbuf[19] == 0x07) && l2capinbuf[20] == 0x05) { #ifdef DEBUG Notify(PSTR("\r\nInactive Wii Motion Plus")); Notify(PSTR("\r\nPlease unplug the Motion Plus, disconnect the Wiimote and then replug the Motion Plus Extension")); @@ -571,7 +572,7 @@ void WII::Run() { Notify(PSTR("\r\nChecking if a Motion Plus is connected")); #endif stateCounter++; - if(stateCounter%100 == 0) + if(stateCounter%200 == 0) checkMotionPresent(); // Check if there is a motion plus connected if(motion_plus_connected_flag) { stateCounter = 0; @@ -586,7 +587,7 @@ void WII::Run() { } } - else if(stateCounter == 301) { // We will try three times to check for the motion plus + else if(stateCounter == 601) { // We will try three times to check for the motion plus #ifdef DEBUG Notify(PSTR("\r\nNo Motion Plus was detected")); #endif @@ -609,7 +610,7 @@ void WII::Run() { else stateCounter = 399; } else if(stateCounter == 200) - initExtension2(); + initExtension2(); else if(stateCounter == 300) { readExtensionType(); unknownExtensionConnected = false; @@ -698,7 +699,10 @@ void WII::Run() { /* HID Commands */ /************************************************************/ void WII::HID_Command(uint8_t* data, uint8_t nbytes) { - pBtd->L2CAP_Command(hci_handle,data,nbytes,control_scid[0],control_scid[1]); // Both the Navigation and Dualshock controller sends data via the control channel + if(pBtd->motionPlusInside) + pBtd->L2CAP_Command(hci_handle,data,nbytes,interrupt_scid[0],interrupt_scid[1]); // It's the new wiimote with the Motion Plus Inside + else + pBtd->L2CAP_Command(hci_handle,data,nbytes,control_scid[0],control_scid[1]); } void WII::setAllOff() { HIDBuffer[1] = 0x11;