Merge pull request #29 from felis/Wii-Pair

The Wii class can now pair with the Wiimote

BTD.cpp

@@ -427,12 +427,18 @@ void BTD::HCI_event_task() {
                 break;
                 
             case EV_PIN_CODE_REQUEST:
-                if(btdPin != NULL) {
+                if(pairWithWii) {
+#ifdef DEBUG
+                    Notify(PSTR("\r\nPairing with wiimote"));
+#endif
+                    hci_pin_code_request_reply();                    
+                }
+                else if(btdPin != NULL) {
 #ifdef DEBUG
                     Notify(PSTR("\r\nBluetooth pin is set too: "));
                     Serial.print(btdPin);
 #endif
-                    hci_pin_code_request_reply(btdPin);
+                    hci_pin_code_request_reply();
                 }
                 else {
 #ifdef DEBUG
@@ -449,6 +455,14 @@ void BTD::HCI_event_task() {
                 hci_link_key_request_negative_reply();
                 break;
                 
+            case EV_AUTHENTICATION_COMPLETE:
+                if(pairWithWii && !connectToWii) {
+#ifdef DEBUG
+                    Notify(PSTR("\r\nPairing successful"));
+#endif
+                    connectToWii = true; // Only send the ACL data to the Wii service
+                }
+                break;
             /* We will just ignore the following events */
             case EV_NUM_COMPLETE_PKT:                
             case EV_ROLE_CHANGED:                
@@ -456,7 +470,6 @@ void BTD::HCI_event_task() {
             case EV_LOOPBACK_COMMAND:                
             case EV_DATA_BUFFER_OVERFLOW:
             case EV_CHANGE_CONNECTION_LINK:
-            case EV_AUTHENTICATION_COMPLETE:                
             case EV_MAX_SLOTS_CHANGE:
             case EV_QOS_SETUP_COMPLETE:
             case EV_LINK_KEY_NOTIFICATION:
@@ -552,16 +565,12 @@ void BTD::HCI_task() {
             break;
             
         case HCI_CHECK_WII_SERVICE:
-            if(wiiServiceID != -1) { // Check if it should try to connect to a wiimote
+            if(pairWithWii) { // Check if it should try to connect to a wiimote
-                if(disc_bdaddr[5] == 0 && disc_bdaddr[4] == 0 && disc_bdaddr[3] == 0 && disc_bdaddr[2] == 0 && disc_bdaddr[1] == 0 && disc_bdaddr[0] == 0) {
 #ifdef DEBUG
-                    Notify(PSTR("\r\nStarting inquiry\r\nPress 1 & 2 on the Wiimote"));
+                Notify(PSTR("\r\nStarting inquiry\r\nPress 1 & 2 on the Wiimote"));
 #endif
-                    hci_inquiry();
+                hci_inquiry();
-                    hci_state = HCI_INQUIRY_STATE;
+                hci_state = HCI_INQUIRY_STATE;
-                }
-                else
-                    hci_state = HCI_CONNECT_WII_STATE;
             }
             else
                 hci_state = HCI_SCANNING_STATE; // Don't try to connect to a Wiimote
@@ -572,23 +581,16 @@ void BTD::HCI_task() {
                 hci_inquiry_cancel(); // Stop inquiry
 #ifdef DEBUG
                 Notify(PSTR("\r\nWiimote found"));
-                Notify(PSTR("\r\nCreate the instance like so to connect automatically:"));
+                Notify(PSTR("\r\nNow just create the instance like so:"));
-                Notify(PSTR("\r\nWII Wii(&Btd,"));
+                Notify(PSTR("\r\nWII Wii(&Btd);"));
-                for(int8_t i = 5; i>0;i--) {
+                Notify(PSTR("\r\nAnd then press any button on the Wiimote"));                
-                    Notify(PSTR("0x"));
-                    PrintHex<uint8_t>(disc_bdaddr[i]);
-                    Notify(PSTR(","));
-                }
-                Notify(PSTR("0x"));
-                PrintHex<uint8_t>(disc_bdaddr[0]);
-                Notify(PSTR(");"));
 #endif                                
                 hci_state = HCI_CONNECT_WII_STATE;
             }
             break;
             
         case HCI_CONNECT_WII_STATE:
-            if(!hci_wii_found || hci_cmd_complete) {
+            if(hci_cmd_complete) {
 #ifdef DEBUG
                 Notify(PSTR("\r\nConnecting to Wiimote"));
 #endif            
@@ -603,7 +605,7 @@ void BTD::HCI_task() {
 #ifdef DEBUG
                     Notify(PSTR("\r\nConnected to Wiimote"));
 #endif                    
-                    connectToWii = true; // Only send the ACL data to the Wii service
+                    hci_authentication_request(); // This will start the pairing with the wiimote
                     hci_state = HCI_SCANNING_STATE;
                 } else {
 #ifdef DEBUG
@@ -615,7 +617,7 @@ void BTD::HCI_task() {
             break;
             
         case HCI_SCANNING_STATE:
-            if(!connectToWii) {
+            if(!connectToWii && !pairWithWii) {
 #ifdef DEBUG
                 Notify(PSTR("\r\nWait For Incoming Connection Request"));
 #endif            
@@ -663,6 +665,12 @@ void BTD::HCI_task() {
                 PrintHex<uint8_t>(disc_bdaddr[0]);
 #endif
                 l2capConnectionClaimed = false;
+                if(strncmp((const char*)remote_name, "Nintendo", 8) == 0) {
+#ifdef DEBUG
+                    Notify(PSTR("\r\nWiimote is connecting"));
+#endif
+                    incomingWii = true;
+                }
                 hci_event_flag = 0;
                 hci_state = HCI_DONE_STATE;
             }
@@ -849,7 +857,7 @@ void BTD::hci_connect() {
     
     HCI_Command(hcibuf, 16);
 }
-void BTD::hci_pin_code_request_reply(const char* key) {
+void BTD::hci_pin_code_request_reply() {
     hcibuf[0] = 0x0D; // HCI OCF = 0D
     hcibuf[1] = 0x01 << 2; // HCI OGF = 1
     hcibuf[2] = 0x17; // parameter length 23
@@ -859,12 +867,24 @@ void BTD::hci_pin_code_request_reply(const char* key) {
     hcibuf[6] = disc_bdaddr[3];
     hcibuf[7] = disc_bdaddr[4];
     hcibuf[8] = disc_bdaddr[5];    
-    hcibuf[9] = strlen(key); // Length of key
+    if(pairWithWii) {
-    uint8_t i;
+        hcibuf[9] = 6; // Pin length is the length of the bt address
-    for(i = 0; i < strlen(key); i++) // The maximum size of the key is 16
+        hcibuf[10] = disc_bdaddr[0]; // The pin is the Wiimotes bt address backwards
-        hcibuf[i+10] = key[i];
+        hcibuf[11] = disc_bdaddr[1];
-    for(;i < 16; i++)
+        hcibuf[12] = disc_bdaddr[2];
-        hcibuf[i+10] = 0x00; // The rest should be 0
+        hcibuf[13] = disc_bdaddr[3];
+        hcibuf[14] = disc_bdaddr[4];
+        hcibuf[15] = disc_bdaddr[5];
+        for(uint8_t i = 16; i < 26; i++)
+            hcibuf[i] = 0x00; // The rest should be 0
+    } else {
+        hcibuf[9] = strlen(btdPin); // Length of pin
+        uint8_t i;
+        for(i = 0; i < strlen(btdPin); i++) // The maximum size of the pin is 16
+            hcibuf[i+10] = btdPin[i];
+        for(;i < 16; i++)
+            hcibuf[i+10] = 0x00; // The rest should be 0
+    }
     
     HCI_Command(hcibuf, 26);
 }
@@ -894,6 +914,15 @@ void BTD::hci_link_key_request_negative_reply() {
     
     HCI_Command(hcibuf, 9);    
 }
+void BTD::hci_authentication_request() {
+    hcibuf[0] = 0x11; // HCI OCF = 11
+    hcibuf[1] = 0x01 << 2; // HCI OGF = 1
+    hcibuf[2] = 0x02; // parameter length = 2
+    hcibuf[3] = (uint8_t)(hci_handle & 0xFF);//connection handle - low byte
+    hcibuf[4] = (uint8_t)((hci_handle >> 8) & 0x0F);//connection handle - high byte
+    
+    HCI_Command(hcibuf, 5);    
+}
 void BTD::hci_disconnect(uint16_t handle) { // This is called by the different services
     hci_event_flag &= ~HCI_FLAG_DISCONN_COMPLETE;
     hcibuf[0] = 0x06; // HCI OCF = 6

BTD.h

@@ -44,7 +44,7 @@
 #define HCI_SET_NAME_STATE      4
 #define HCI_CHECK_WII_SERVICE   5
 
-#define HCI_INQUIRY_STATE       6 // These three states are only used if it should connect to a Wii controller
+#define HCI_INQUIRY_STATE       6 // These three states are only used if it should pair and connect to a Wii controller
 #define HCI_CONNECT_WII_STATE   7
 #define HCI_CONNECTED_WII_STATE 8
 
@@ -182,7 +182,10 @@ public:
     uint8_t hci_version;
     
     int8_t wiiServiceID; // Stores the service ID of the Wii service
+    
     bool connectToWii; // Used to only send the ACL data to the wiimote
+    bool incomingWii;
+    bool pairWithWii;
     
     /* HCI Commands */
     void HCI_Command(uint8_t* data, uint16_t nbytes);
@@ -195,9 +198,10 @@ public:
     void hci_accept_connection();
     void hci_write_scan_disable();
     void hci_disconnect(uint16_t handle);
-    void hci_pin_code_request_reply(const char* key);
+    void hci_pin_code_request_reply();
     void hci_pin_code_negative_request_reply();
     void hci_link_key_request_negative_reply();
+    void hci_authentication_request();
     void hci_inquiry();
     void hci_inquiry_cancel();
     void hci_connect();

Wii.cpp

@@ -20,20 +20,15 @@
 //#define EXTRADEBUG // Uncomment to get even more debugging data
 //#define PRINTREPORT // Uncomment to print the report send by the Wii controllers
 
-WII::WII(BTD *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0):
+WII::WII(BTD *p, bool pair):
 pBtd(p) // pointer to USB class instance - mandatory
 {
     if (pBtd)
         pBtd->wiiServiceID = pBtd->registerServiceClass(this); // Register it as a Bluetooth service
     
-    pBtd->disc_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead
+    pBtd->pairWithWii = pair;
-    pBtd->disc_bdaddr[4] = btadr4;
-    pBtd->disc_bdaddr[3] = btadr3;
-    pBtd->disc_bdaddr[2] = btadr2;
-    pBtd->disc_bdaddr[1] = btadr1;
-    pBtd->disc_bdaddr[0] = btadr0;
             
-    HIDBuffer[0] = 0xA2;// HID BT DATA_request (0x50) | Report Type (Output 0x02)
+    HIDBuffer[0] = 0xA2;// HID BT DATA_request (0xA0) | Report Type (Output 0x02)
     
     /* Set device cid for the control and intterrupt channelse - LSB */
     control_dcid[0] = 0x60;//0x0060
@@ -47,6 +42,8 @@ void WII::Reset() {
     wiimoteConnected = false;
     nunchuckConnected = false;
     motionPlusConnected = false;
+    activateNunchuck = false;
+    motionValuesReset = false;
     l2cap_event_flag = 0; // Reset flags
     l2cap_state = L2CAP_WAIT;
 }
@@ -59,7 +56,7 @@ void WII::disconnect() { // Use this void to disconnect any of the controllers
 }
 
 void WII::ACLData(uint8_t* l2capinbuf) {
-    if (((l2capinbuf[0] | (l2capinbuf[1] << 8)) == (hci_handle | 0x2000))) { //acl_handle_ok
+    if (((l2capinbuf[0] | (l2capinbuf[1] << 8)) == (hci_handle | 0x2000)) || pBtd->incomingWii) { // acl_handle_ok or it's a new connection
         if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U
             if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
 #ifdef DEBUG
@@ -95,6 +92,32 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                     }
                 }
             }             
+            else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
+#ifdef EXTRADEBUG
+                Notify(PSTR("\r\nL2CAP Connection Request - PSM: "));
+                PrintHex<uint8_t>(l2capinbuf[13]);
+                Notify(PSTR(" "));
+                PrintHex<uint8_t>(l2capinbuf[12]);
+                Notify(PSTR(" SCID: "));
+                PrintHex<uint8_t>(l2capinbuf[15]);
+                Notify(PSTR(" "));
+                PrintHex<uint8_t>(l2capinbuf[14]);
+                Notify(PSTR(" Identifier: "));
+                PrintHex<uint8_t>(l2capinbuf[9]);
+#endif
+                if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
+                    identifier = l2capinbuf[9];
+                    control_scid[0] = l2capinbuf[14];
+                    control_scid[1] = l2capinbuf[15];
+                    l2cap_event_flag |= L2CAP_FLAG_CONNECTION_CONTROL_REQUEST;
+                }
+                else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) {
+                    identifier = l2capinbuf[9];
+                    interrupt_scid[0] = l2capinbuf[14];
+                    interrupt_scid[1] = l2capinbuf[15];
+                    l2cap_event_flag |= L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST;
+                }
+            }
             else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
                 if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
                     if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
@@ -251,6 +274,12 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                     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) {
+#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"));
+#endif
+                                    stateCounter = 300; // Skip the rest in "L2CAP_CHECK_MOTION_PLUS_STATE"
                                 }
 #ifdef DEBUG
                                 else {
@@ -410,6 +439,33 @@ void WII::ACLData(uint8_t* l2capinbuf) {
 }
 void WII::L2CAP_task() {
     switch (l2cap_state) {
+        /* These states are used if the Wiimote is the host */
+        case L2CAP_CONTROL_SUCCESS:
+            if (l2cap_config_success_control_flag) {
+#ifdef DEBUG
+                Notify(PSTR("\r\nHID Control Successfully Configured"));
+#endif
+                l2cap_state = L2CAP_INTERRUPT_SETUP;
+            }
+            break;
+            
+        case L2CAP_INTERRUPT_SETUP:
+            if (l2cap_connection_request_interrupt_flag) {
+#ifdef DEBUG
+                Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"));
+#endif
+                pBtd->l2cap_connection_response(hci_handle,identifier, interrupt_dcid, interrupt_scid, PENDING);
+                delay(1);
+                pBtd->l2cap_connection_response(hci_handle,identifier, interrupt_dcid, interrupt_scid, SUCCESSFUL);
+                identifier++;
+                delay(1);
+                pBtd->l2cap_config_request(hci_handle,identifier, interrupt_scid);
+                
+                l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST;
+            }
+            break;
+
+        /* These states are used if the Arduino is the host */
         case L2CAP_CONTROL_CONNECT_REQUEST:
             if (l2cap_connected_control_flag) {
 #ifdef DEBUG
@@ -443,13 +499,13 @@ void WII::L2CAP_task() {
             }
             break;
             
-            
         case L2CAP_INTERRUPT_CONFIG_REQUEST:
-            if(l2cap_config_success_interrupt_flag) {
+            if(l2cap_config_success_interrupt_flag) { // Now the HID channels is established
 #ifdef DEBUG
                 Notify(PSTR("\r\nHID Channels Established"));
 #endif
                 pBtd->connectToWii = false;
+                pBtd->pairWithWii = false;
                 wiimoteConnected = true;
                 stateCounter = 0;
                 l2cap_state = L2CAP_CHECK_MOTION_PLUS_STATE;
@@ -493,11 +549,22 @@ void WII::Run() {
                 identifier = 0;
                 pBtd->l2cap_connection_request(hci_handle,identifier,control_dcid,HID_CTRL_PSM);
                 l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST;                
+            } else if (l2cap_connection_request_control_flag) {
+                hci_handle = pBtd->hci_handle;
+                pBtd->incomingWii = false;
+#ifdef DEBUG
+                Notify(PSTR("\r\nHID Control Incoming Connection Request"));
+#endif
+                pBtd->l2cap_connection_response(hci_handle,identifier, control_dcid, control_scid, PENDING);
+                delay(1);
+                pBtd->l2cap_connection_response(hci_handle,identifier, control_dcid, control_scid, SUCCESSFUL);
+                identifier++;
+                delay(1);
+                pBtd->l2cap_config_request(hci_handle,identifier, control_scid);
+                l2cap_state = L2CAP_CONTROL_SUCCESS;
             }
             break;           
             
-            
-            
         case L2CAP_CHECK_MOTION_PLUS_STATE:
 #ifdef DEBUG
             if(stateCounter == 0) // Only print onnce

Wii.h

@@ -22,19 +22,26 @@
 
 /* Bluetooth L2CAP states for L2CAP_task() */
 #define L2CAP_WAIT                      0
-#define L2CAP_CONTROL_CONNECT_REQUEST   1
-#define L2CAP_CONTROL_CONFIG_REQUEST    2
-#define L2CAP_INTERRUPT_CONNECT_REQUEST 3
-#define L2CAP_INTERRUPT_CONFIG_REQUEST  4
 
-#define L2CAP_CHECK_MOTION_PLUS_STATE   5
+// These states are used if the Wiimote is the host
-#define L2CAP_CHECK_EXTENSION_STATE     6
+#define L2CAP_CONTROL_SUCCESS           1
-#define L2CAP_INIT_MOTION_PLUS_STATE    7
+#define L2CAP_INTERRUPT_SETUP           2
 
-#define L2CAP_LED_STATE                 8
+// These states are used if the Arduino is the host
-#define L2CAP_DONE                      9
+#define L2CAP_CONTROL_CONNECT_REQUEST   3
-#define L2CAP_INTERRUPT_DISCONNECT      10
+#define L2CAP_CONTROL_CONFIG_REQUEST    4
-#define L2CAP_CONTROL_DISCONNECT        11
+#define L2CAP_INTERRUPT_CONNECT_REQUEST 5
+ 
+#define L2CAP_INTERRUPT_CONFIG_REQUEST  6
+
+#define L2CAP_CHECK_MOTION_PLUS_STATE   7
+#define L2CAP_CHECK_EXTENSION_STATE     8
+#define L2CAP_INIT_MOTION_PLUS_STATE    9
+#define L2CAP_LED_STATE                 10
+#define L2CAP_DONE                      11
+
+#define L2CAP_INTERRUPT_DISCONNECT      12
+#define L2CAP_CONTROL_DISCONNECT        13
 
 /* L2CAP event flags */
 #define L2CAP_FLAG_CONTROL_CONNECTED                0x001
@@ -43,6 +50,8 @@
 #define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS         0x008
 #define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE      0x040
 #define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE    0x080
+#define L2CAP_FLAG_CONNECTION_CONTROL_REQUEST       0x100
+#define L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST     0x200
 
 /* Macros for L2CAP event flag tests */
 #define l2cap_connected_control_flag (l2cap_event_flag & L2CAP_FLAG_CONTROL_CONNECTED)
@@ -51,14 +60,18 @@
 #define l2cap_config_success_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS)
 #define l2cap_disconnect_response_control_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE)
 #define l2cap_disconnect_response_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE)
+#define l2cap_connection_request_control_flag (l2cap_event_flag & L2CAP_FLAG_CONNECTION_CONTROL_REQUEST)
+#define l2cap_connection_request_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST)
 
 /* Wii event flags */
-#define WII_FLAG_MOTION_PLUS_CONNECTED              0x100
+#define WII_FLAG_MOTION_PLUS_CONNECTED              0x400
-#define WII_FLAG_NUNCHUCK_CONNECTED                 0x200
+#define WII_FLAG_NUNCHUCK_CONNECTED                 0x800
 
 #define motion_plus_connected_flag (l2cap_event_flag & WII_FLAG_MOTION_PLUS_CONNECTED)
 #define nunchuck_connected_flag (l2cap_event_flag & WII_FLAG_NUNCHUCK_CONNECTED)
 
+#define PAIR    1
+
 enum LED {
     LED1 = 0x10,
     LED2 = 0x20,
@@ -97,7 +110,7 @@ enum AnalogHat {
 
 class WII : public BluetoothService {
 public:
-    WII(BTD *pBtd, uint8_t btadr5=0, uint8_t btadr4=0, uint8_t btadr3=0, uint8_t btadr2=0, uint8_t btadr1=0, uint8_t btadr0=0);
+    WII(BTD *p, bool pair=false);
     
     // BluetoothService implementation
     virtual void ACLData(uint8_t* ACLData); // Used to pass acldata to the services

examples/Bluetooth/Wii/Wii.ino

@@ -8,8 +8,8 @@
 USB Usb;
 BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
 /* You can create the instance of the class in two ways */
-WII Wii(&Btd); // This will start inquiry which will connect to any Wiimote
+WII Wii(&Btd,PAIR); // This will start an inquiry and then pair with your Wiimote - you only have to do this once
-//WII Wii(&Btd,0x00,0x26,0x59,0x48,0xFF,0xFB); // This will connect to the Wiimote with that specific Bluetooth Address
+//WII Wii(&Btd); // After that you can simply create the instance like so and then press any button on the Wiimote
 
 bool printAngle;
 

keywords.txt

@@ -223,3 +223,4 @@ setRumbleToggle			KEYWORD2
 getPitch				KEYWORD2
 getRoll					KEYWORD2
 getYaw					KEYWORD2
+PAIR					KEYWORD2