Fixed disconnect problem when using the old Wiimote with the external MotionPlus extension

Now also updates the batteryLevel when getBatteryLevel is called

Wii.cpp

@@ -14,9 +14,7 @@
  Web      :  http://www.tkjelectronics.com
  e-mail   :  kristianl@tkjelectronics.com
 
- IR camera support added by:
+ IR camera support added by Allan Glover (adglover9.81@gmail.com) and Kristian Lauszus
- Allan Glover
- adglover9.81@gmail.com
  */
 
 #include "Wii.h"
@@ -109,7 +107,7 @@ void WII::Reset() {
         activateNunchuck = false;
         motionValuesReset = false;
         activeConnection = false;
-        pBtd->motionPlusInside = false;
+        motionPlusInside = false;
         pBtd->wiiUProController = false;
         wiiUProControllerConnected = false;
         l2cap_event_flag = 0; // Reset flags
@@ -117,13 +115,17 @@ void WII::Reset() {
 }
 
 void WII::disconnect() { // Use this void to disconnect any of the controllers
-        if (motionPlusConnected && !pBtd->motionPlusInside) { // Disable the Motion Plus extension
+        if (!motionPlusInside) { // The old Wiimote needs a delay after the first command or it will automatically reconnect
+                if (motionPlusConnected) {
 #ifdef DEBUG_USB_HOST
                         Notify(PSTR("\r\nDeactivating Motion Plus"), 0x80);
 #endif
                         initExtension1(); // This will disable the Motion Plus extension
                 }
-        //First the HID interrupt channel has to be disconencted, then the HID control channel and finally the HCI connection
+                timer = millis() + 1000; // We have to wait for the message before the rest of the channels can be deactivated
+        } else
+                timer = millis(); // Don't wait
+        // First the HID interrupt channel has to be disconnected, then the HID control channel and finally the HCI connection
         pBtd->l2cap_disconnection_request(hci_handle, 0x0A, interrupt_scid, interrupt_dcid);
         Reset();
         l2cap_state = L2CAP_INTERRUPT_DISCONNECT;
@@ -133,6 +135,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
         if (!pBtd->l2capConnectionClaimed && pBtd->incomingWii && !wiimoteConnected && !activeConnection) {
                 if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
                         if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
+                                motionPlusInside = pBtd->motionPlusInside;
                                 pBtd->incomingWii = false;
                                 pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service
                                 activeConnection = true;
@@ -254,7 +257,6 @@ void WII::ACLData(uint8_t* l2capinbuf) {
 #endif
                 } else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
                         //Notify(PSTR("\r\nL2CAP Interrupt"), 0x80);
-                        if (wiimoteConnected) {
                                 if (l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
                                         if ((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || (l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons
                                                 if ((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33) // These reports have no extensions bytes
@@ -291,13 +293,16 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                         }
                                         switch (l2capinbuf[9]) {
                                                 case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV
+#ifdef EXTRADEBUG
+                                                        Notify(PSTR("\r\nStatus report was received"), 0x80);
+#endif
                                                         wiiState = l2capinbuf[12]; // (0x01: Battery is nearly empty), (0x02:  An Extension Controller is connected), (0x04: Speaker enabled), (0x08: IR enabled), (0x10: LED1, 0x20: LED2, 0x40: LED3, 0x80: LED4)
                                                         batteryLevel = l2capinbuf[15]; // Update battery level
-                                                        if (l2capinbuf[12] & 0x01) {
 #ifdef DEBUG_USB_HOST
+                                                        if (l2capinbuf[12] & 0x01)
                                                                 Notify(PSTR("\r\nWARNING: Battery is nearly empty"), 0x80);
 #endif
-                                                        }
+                                                        if (checkExtension) { // If this is false it means that the user must have called getBatteryLevel()
                                                                 if (l2capinbuf[12] & 0x02) { // Check if a extension is connected
 #ifdef DEBUG_USB_HOST
                                                                         if (!unknownExtensionConnected)
@@ -327,11 +332,12 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                                                                 nunchuckConnected = false; // It must be the Nunchuck controller then
                                                                                 l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED;
                                                                                 onInit();
-                                                                        setReportMode(false, 0x31); // If there is no extension connected we will read the button and accelerometer
+                                                                                setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer
-                                                                } else {
+                                                                        } else
-                                                                        setReportMode(false, 0x31); // If there is no extension connected we will read the button and accelerometer
+                                                                                setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer
-                                                                }
                                                                 }
+                                                        } else
+                                                                checkExtension = true; // Check for extensions by default
                                                         break;
                                                 case 0x21: // Read Memory Data
                                                         if ((l2capinbuf[12] & 0x0F) == 0) { // No error
@@ -351,6 +357,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                                                         Notify(PSTR("\r\nMotion Plus activated in normal mode"), 0x80);
 #endif
                                                                         motionPlusConnected = true;
+                                                                        setReportMode(false, 0x35); // Also read the extension
                                                                 } else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) {
 #ifdef DEBUG_USB_HOST
                                                                         Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode"), 0x80);
@@ -358,6 +365,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                                                         activateNunchuck = false;
                                                                         motionPlusConnected = true;
                                                                         nunchuckConnected = true;
+                                                                        setReportMode(false, 0x35); // Also read the extension
                                                                 } 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_USB_HOST
                                                                         Notify(PSTR("\r\nInactive Wii Motion Plus"), 0x80);
@@ -573,7 +581,6 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                         }
                                 }
                 }
-                }
                 L2CAP_task();
         }
 }
@@ -647,7 +654,6 @@ void WII::L2CAP_task() {
 #endif
                                 pBtd->connectToWii = false;
                                 pBtd->pairWithWii = false;
-                                wiimoteConnected = true;
                                 stateCounter = 0;
                                 l2cap_state = L2CAP_CHECK_MOTION_PLUS_STATE;
                         }
@@ -656,7 +662,7 @@ void WII::L2CAP_task() {
                         /* The next states are in run() */
 
                 case L2CAP_INTERRUPT_DISCONNECT:
-                        if (l2cap_disconnect_response_interrupt_flag) {
+                        if (l2cap_disconnect_response_interrupt_flag && millis() > timer) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80);
 #endif
@@ -681,11 +687,15 @@ void WII::L2CAP_task() {
 }
 
 void WII::Run() {
+        if (l2cap_state == L2CAP_INTERRUPT_DISCONNECT && millis() > timer)
+                L2CAP_task(); // Call the rest of the disconnection routine after we have waited long enough
+
         switch (l2cap_state) {
                 case L2CAP_WAIT:
                         if (pBtd->connectToWii && !pBtd->l2capConnectionClaimed && !wiimoteConnected && !activeConnection) {
                                 pBtd->l2capConnectionClaimed = true;
                                 activeConnection = true;
+                                motionPlusInside = pBtd->motionPlusInside;
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nSend HID Control Connection Request"), 0x80);
 #endif
@@ -779,6 +789,7 @@ void WII::Run() {
                 case L2CAP_LED_STATE:
                         if (nunchuck_connected_flag)
                                 nunchuckConnected = true;
+                        wiimoteConnected = true;
                         onInit();
                         l2cap_state = L2CAP_DONE;
                         break;
@@ -839,7 +850,7 @@ void WII::Run() {
 
 /************************************************************/
 void WII::HID_Command(uint8_t* data, uint8_t nbytes) {
-        if (pBtd->motionPlusInside)
+        if (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]);
@@ -905,6 +916,12 @@ void WII::setLedStatus() {
         HID_Command(HIDBuffer, 3);
 }
 
+uint8_t WII::getBatteryLevel() {
+        checkExtension = false; // This is needed so the library knows that the status response is a response to this function
+        statusRequest(); // This will update the battery level
+        return batteryLevel;
+};
+
 void WII::setReportMode(bool continuous, uint8_t mode) {
         uint8_t cmd_buf[4];
         cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)

Wii.h

@@ -14,9 +14,7 @@
  Web      :  http://www.tkjelectronics.com
  e-mail   :  kristianl@tkjelectronics.com
 
- IR camera support added by:
+ IR camera support added by Allan Glover (adglover9.81@gmail.com) and Kristian Lauszus
- Allan Glover
- adglover9.81@gmail.com
  */
 
 #ifndef _wii_h_
@@ -215,17 +213,11 @@ public:
          */
         void setLedStatus();
 
-        /**
-         * Call this to update battery level and Wiimote state
-         */
-        void statusRequest();
         /**
          * Return the battery level of the Wiimote.
          * @return The battery level in the range 0-255.
          */
-        uint8_t getBatteryLevel() {
+        uint8_t getBatteryLevel();
-                return batteryLevel;
-        };
         /**
          * Return the Wiimote state.
          * @return See: http://wiibrew.org/wiki/Wiimote#0x20:_Status.
@@ -456,6 +448,8 @@ private:
         uint16_t stateCounter;
         bool unknownExtensionConnected;
         bool extensionConnected;
+        bool checkExtension; // Set to false when getBatteryLevel() is called otherwise if should be true
+        bool motionPlusInside; // True if it's a new Wiimote with the Motion Plus extension build into it
 
         /* L2CAP Channels */
         uint8_t control_scid[2]; // L2CAP source CID for HID_Control
@@ -472,6 +466,8 @@ private:
         void initExtension1();
         void initExtension2();
 
+        void statusRequest(); // Used to update the Wiimote state and battery level
+
         void readData(uint32_t offset, uint16_t size, bool EEPROM);
         void readExtensionType();
         void readCalData();
@@ -491,7 +487,7 @@ private:
         uint8_t batteryLevel;
 
 #ifdef WIICAMERA
-        /* Private function and variables for the readings from teh IR Camera */
+        /* Private function and variables for the readings from the IR Camera */
         void enableIRCamera1(); // Sets bit 2 of output report 13
         void enableIRCamera2(); // Sets bit 2 of output report 1A
         void writeSensitivityBlock1();

examples/Bluetooth/WiiIRCamera/WiiIRCamera.ino

@@ -2,13 +2,13 @@
 Example sketch for the Wii libary showing the IR camera functionality. This example
 is for the Bluetooth Wii library developed for the USB shield from Circuits@Home
  
-Created by Allan Glover and includes much from what Kristian Lauszus wrote in the existing 
+Created by Allan Glover and Kristian Lauszus.
-Wii example. Contact Kristian:  http://blog.tkjelectronics.dk/ or send email at kristianl@tkjelectronics.com.
+Contact Kristian:  http://blog.tkjelectronics.dk/ or send an email at kristianl@tkjelectronics.com.
 Contact Allan at adglover9.81@gmail.com
  
 To test the Wiimote IR camera, you will need access to an IR source. Sunlight will work but is not ideal. 
-The simpleist solution is to use the Wii sensor bar, i.e. emitter bar, supplied by the Wii system. Otherwise,
+The simpleist solution is to use the Wii sensor bar, i.e. emitter bar, supplied by the Wii system.
-wire up a IR LED yourself.
+Otherwise, wire up a IR LED yourself.
 */
 
 #include <Wii.h>
@@ -19,7 +19,7 @@ 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,PAIR); // This will start an inquiry and then pair with your Wiimote - you only have to do this once
-//WII Wii(&Btd); // After the wiimote pairs once with the line of code above, you can simply create the instance like so and re upload and then press any button on the Wiimote
+//WII Wii(&Btd); // After the Wiimote pairs once with the line of code above, you can simply create the instance like so and re upload and then press any button on the Wiimote
 
 bool printAngle;
 uint8_t printObjects;
@@ -109,7 +109,7 @@ void loop() {
         }
       }
     } 
-    if(printAngle) { // There is no extension bytes avaliable, so the Motionplus or Nunchuck can't be read
+    if(printAngle) { // There is no extension bytes available, so the MotionPlus or Nunchuck can't be read
       Serial.print(F("\r\nPitch: "));
       Serial.print(Wii.getPitch());
       Serial.print(F("\tRoll: "));

examples/Bluetooth/WiiMulti/WiiMulti.ino

@@ -35,7 +35,6 @@ void loop() {
         Serial.print(F("\r\nHOME"));
         Wii[i]->disconnect();
         oldControllerState[i] = false; // Reset value
-        delay(1000); // This delay is needed for some Wiimotes, so it doesn't try to reconnect right away
       } 
       else {
         if(Wii[i]->getButtonClick(LEFT)) {