Now able to pair with 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) {
+#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,8 @@ 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
+                    //connectToWii = true;
                     hci_state = HCI_SCANNING_STATE;
                 } else {
 #ifdef DEBUG
@@ -615,7 +618,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 +666,10 @@ void BTD::HCI_task() {
                 PrintHex<uint8_t>(disc_bdaddr[0]);
 #endif
                 l2capConnectionClaimed = false;
+                if(strncmp((const char *)remote_name, "Nintendo", 8) == 0) {
+                    Serial.print("\r\nWiimote is connecting");
+                    incomingWii = true;
+                }
                 hci_event_flag = 0;
                 hci_state = HCI_DONE_STATE;
             }
@@ -849,7 +856,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,13 +866,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[5]; // The pin is the Wiimotes bt address backwards
-        hcibuf[i+10] = key[i];
+        hcibuf[11] = disc_bdaddr[4];
-    for(;i < 16; i++)
+        hcibuf[12] = disc_bdaddr[3];
-        hcibuf[i+10] = 0x00; // The rest should be 0
+        hcibuf[13] = disc_bdaddr[2];
-    
+        hcibuf[14] = disc_bdaddr[1];
+        hcibuf[15] = disc_bdaddr[0];
+        for(uint8_t i = 16; i < 26; i++)
+            hcibuf[i] = 0x00; // The rest should be 0
+    } else {
+        hcibuf[9] = strlen(btdPin); // Length of key
+        uint8_t i;
+        for(i = 0; i < strlen(btdPin); i++) // The maximum size of the key is 16
+            hcibuf[i+10] = btdPin[i];
+        for(;i < 16; i++)
+            hcibuf[i+10] = 0x00; // The rest should be 0
+    }
     HCI_Command(hcibuf, 26);
 }
 void BTD::hci_pin_code_negative_request_reply() {
@@ -894,6 +912,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,18 +20,13 @@
 //#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)
     
@@ -47,6 +42,7 @@ void WII::Reset() {
     wiimoteConnected = false;
     nunchuckConnected = false;
     motionPlusConnected = false;
+    activateNunchuck = false;
     l2cap_event_flag = 0; // Reset flags
     l2cap_state = L2CAP_WAIT;
 }
@@ -59,7 +55,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 +91,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]) {
@@ -410,6 +432,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 +492,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 +542,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
@@ -552,7 +612,7 @@ void WII::Run() {
             }
             break;
             
-        case L2CAP_INIT_MOTION_PLUS_STATE:
+        case L2CAP_INIT_MOTION_PLUS_STATE: {
             stateCounter++;
             if(stateCounter == 1)
                 initMotionPlus();
@@ -566,13 +626,15 @@ void WII::Run() {
                 l2cap_state = L2CAP_LED_STATE;
             }
             break;
+        }
             
-        case L2CAP_LED_STATE:
+        case L2CAP_LED_STATE: {
             if(nunchuck_connected_flag)
                 nunchuckConnected = true;
             setLedStatus();
             l2cap_state = L2CAP_DONE;
             break;            
+        }
              
         case L2CAP_DONE:
             if(unknownExtensionConnected) {

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

keywords.txt

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