tmk_core/protocol/usb_hid/USB_Host_Shield_2.0/Wii.cpp

   1 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
   2
   3  This software may be distributed and modified under the terms of the GNU
   4  General Public License version 2 (GPL2) as published by the Free Software
   5  Foundation and appearing in the file GPL2.TXT included in the packaging of
   6  this file. Please note that GPL2 Section 2[b] requires that all works based
   7  on this software must also be made publicly available under the terms of
   8  the GPL2 ("Copyleft").
   9
  10  Contact information
  11  -------------------
  12
  13  Kristian Lauszus, TKJ Electronics
  14  Web      :  http://www.tkjelectronics.com
  15  e-mail   :  kristianl@tkjelectronics.com
  16
  17  IR camera support added by Allan Glover (adglover9.81@gmail.com) and Kristian Lauszus
  18  */
  19
  20 #include "Wii.h"
  21 // To enable serial debugging see "settings.h"
  22 //#define EXTRADEBUG // Uncomment to get even more debugging data
  23 //#define PRINTREPORT // Uncomment to print the report send by the Wii controllers
  24
  25 const uint8_t WII_LEDS[] PROGMEM = {
  26         0x00, // OFF
  27         0x10, // LED1
  28         0x20, // LED2
  29         0x40, // LED3
  30         0x80, // LED4
  31
  32         0x90, // LED5
  33         0xA0, // LED6
  34         0xC0, // LED7
  35         0xD0, // LED8
  36         0xE0, // LED9
  37         0xF0, // LED10
  38 };
  39
  40 const uint32_t WII_BUTTONS[] PROGMEM = {
  41         0x00008, // UP
  42         0x00002, // RIGHT
  43         0x00004, // DOWN
  44         0x00001, // LEFT
  45
  46         0, // Skip
  47         0x00010, // PLUS
  48         0x00100, // TWO
  49         0x00200, // ONE
  50
  51         0x01000, // MINUS
  52         0x08000, // HOME
  53         0x10000, // Z
  54         0x20000, // C
  55
  56         0x00400, // B
  57         0x00800, // A
  58 };
  59 const uint32_t WII_PROCONTROLLER_BUTTONS[] PROGMEM = {
  60         0x00100, // UP
  61         0x00080, // RIGHT
  62         0x00040, // DOWN
  63         0x00200, // LEFT
  64
  65         0, // Skip
  66         0x00004, // PLUS
  67         0x20000, // L3
  68         0x10000, // R3
  69
  70         0x00010, // MINUS
  71         0x00008, // HOME
  72         0, 0, // Skip
  73
  74         0x04000, // B
  75         0x01000, // A
  76         0x00800, // X
  77         0x02000, // Y
  78
  79         0x00020, // L
  80         0x00002, // R
  81         0x08000, // ZL
  82         0x00400, // ZR
  83 };
  84
  85 WII::WII(BTD *p, bool pair) :
  86 BluetoothService(p) // Pointer to USB class instance - mandatory
  87 {
  88         pBtd->pairWithWii = pair;
  89
  90         HIDBuffer[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
  91
  92         /* Set device cid for the control and intterrupt channelse - LSB */
  93         control_dcid[0] = 0x60; // 0x0060
  94         control_dcid[1] = 0x00;
  95         interrupt_dcid[0] = 0x61; // 0x0061
  96         interrupt_dcid[1] = 0x00;
  97
  98         Reset();
  99 }
 100
 101 void WII::Reset() {
 102         wiimoteConnected = false;
 103         nunchuckConnected = false;
 104         motionPlusConnected = false;
 105         activateNunchuck = false;
 106         motionValuesReset = false;
 107         activeConnection = false;
 108         motionPlusInside = false;
 109         pBtd->wiiUProController = false;
 110         wiiUProControllerConnected = false;
 111         wiiBalanceBoardConnected = false;
 112         l2cap_event_flag = 0; // Reset flags
 113         l2cap_state = L2CAP_WAIT;
 114 }
 115
 116 void WII::disconnect() { // Use this void to disconnect any of the controllers
 117         if(!motionPlusInside) { // The old Wiimote needs a delay after the first command or it will automatically reconnect
 118                 if(motionPlusConnected) {
 119 #ifdef DEBUG_USB_HOST
 120                         Notify(PSTR("\r\nDeactivating Motion Plus"), 0x80);
 121 #endif
 122                         initExtension1(); // This will disable the Motion Plus extension
 123                 }
 124                 timer = millis() + 1000; // We have to wait for the message before the rest of the channels can be deactivated
 125         } else
 126                 timer = millis(); // Don't wait
 127         // First the HID interrupt channel has to be disconnected, then the HID control channel and finally the HCI connection
 128         pBtd->l2cap_disconnection_request(hci_handle, ++identifier, interrupt_scid, interrupt_dcid);
 129         Reset();
 130         l2cap_state = L2CAP_INTERRUPT_DISCONNECT;
 131 }
 132
 133 void WII::ACLData(uint8_t* l2capinbuf) {
 134         if(!pBtd->l2capConnectionClaimed && pBtd->incomingWii && !wiimoteConnected && !activeConnection) {
 135                 if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
 136                         if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
 137                                 motionPlusInside = pBtd->motionPlusInside;
 138                                 pBtd->incomingWii = false;
 139                                 pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service
 140                                 activeConnection = true;
 141                                 hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
 142                                 l2cap_state = L2CAP_WAIT;
 143                         }
 144                 }
 145         }
 146
 147         if(checkHciHandle(l2capinbuf, hci_handle)) { // acl_handle_ok
 148                 if((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001U) { // l2cap_control - Channel ID for ACL-U
 149                         if(l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
 150 #ifdef DEBUG_USB_HOST
 151                                 Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80);
 152                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
 153                                 Notify(PSTR(" "), 0x80);
 154                                 D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
 155                                 Notify(PSTR(" "), 0x80);
 156                                 D_PrintHex<uint8_t > (l2capinbuf[17], 0x80);
 157                                 Notify(PSTR(" "), 0x80);
 158                                 D_PrintHex<uint8_t > (l2capinbuf[16], 0x80);
 159                                 Notify(PSTR(" "), 0x80);
 160                                 D_PrintHex<uint8_t > (l2capinbuf[15], 0x80);
 161                                 Notify(PSTR(" "), 0x80);
 162                                 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
 163 #endif
 164                         } else if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_RESPONSE) {
 165                                 if(((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) && ((l2capinbuf[18] | (l2capinbuf[19] << 8)) == SUCCESSFUL)) { // Success
 166                                         if(l2capinbuf[14] == control_dcid[0] && l2capinbuf[15] == control_dcid[1]) {
 167                                                 //Notify(PSTR("\r\nHID Control Connection Complete"), 0x80);
 168                                                 identifier = l2capinbuf[9];
 169                                                 control_scid[0] = l2capinbuf[12];
 170                                                 control_scid[1] = l2capinbuf[13];
 171                                                 l2cap_set_flag(L2CAP_FLAG_CONTROL_CONNECTED);
 172                                         } else if(l2capinbuf[14] == interrupt_dcid[0] && l2capinbuf[15] == interrupt_dcid[1]) {
 173                                                 //Notify(PSTR("\r\nHID Interrupt Connection Complete"), 0x80);
 174                                                 identifier = l2capinbuf[9];
 175                                                 interrupt_scid[0] = l2capinbuf[12];
 176                                                 interrupt_scid[1] = l2capinbuf[13];
 177                                                 l2cap_set_flag(L2CAP_FLAG_INTERRUPT_CONNECTED);
 178                                         }
 179                                 }
 180                         } else if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
 181 #ifdef EXTRADEBUG
 182                                 Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80);
 183                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
 184                                 Notify(PSTR(" "), 0x80);
 185                                 D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
 186                                 Notify(PSTR(" SCID: "), 0x80);
 187                                 D_PrintHex<uint8_t > (l2capinbuf[15], 0x80);
 188                                 Notify(PSTR(" "), 0x80);
 189                                 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
 190                                 Notify(PSTR(" Identifier: "), 0x80);
 191                                 D_PrintHex<uint8_t > (l2capinbuf[9], 0x80);
 192 #endif
 193                                 if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
 194                                         identifier = l2capinbuf[9];
 195                                         control_scid[0] = l2capinbuf[14];
 196                                         control_scid[1] = l2capinbuf[15];
 197                                         l2cap_set_flag(L2CAP_FLAG_CONNECTION_CONTROL_REQUEST);
 198                                 } else if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) {
 199                                         identifier = l2capinbuf[9];
 200                                         interrupt_scid[0] = l2capinbuf[14];
 201                                         interrupt_scid[1] = l2capinbuf[15];
 202                                         l2cap_set_flag(L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST);
 203                                 }
 204                         } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
 205                                 if((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
 206                                         if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
 207                                                 //Notify(PSTR("\r\nHID Control Configuration Complete"), 0x80);
 208                                                 identifier = l2capinbuf[9];
 209                                                 l2cap_set_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS);
 210                                         } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
 211                                                 //Notify(PSTR("\r\nHID Interrupt Configuration Complete"), 0x80);
 212                                                 identifier = l2capinbuf[9];
 213                                                 l2cap_set_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS);
 214                                         }
 215                                 }
 216                         } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
 217                                 if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
 218                                         //Notify(PSTR("\r\nHID Control Configuration Request"), 0x80);
 219                                         pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], control_scid);
 220                                 } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
 221                                         //Notify(PSTR("\r\nHID Interrupt Configuration Request"), 0x80);
 222                                         pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], interrupt_scid);
 223                                 }
 224                         } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
 225                                 if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
 226 #ifdef DEBUG_USB_HOST
 227                                         Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80);
 228 #endif
 229                                         identifier = l2capinbuf[9];
 230                                         pBtd->l2cap_disconnection_response(hci_handle, identifier, control_dcid, control_scid);
 231                                         Reset();
 232                                 } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
 233 #ifdef DEBUG_USB_HOST
 234                                         Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80);
 235 #endif
 236                                         identifier = l2capinbuf[9];
 237                                         pBtd->l2cap_disconnection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid);
 238                                         Reset();
 239                                 }
 240                         } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
 241                                 if(l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
 242                                         //Notify(PSTR("\r\nDisconnect Response: Control Channel"), 0x80);
 243                                         identifier = l2capinbuf[9];
 244                                         l2cap_set_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE);
 245                                 } else if(l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) {
 246                                         //Notify(PSTR("\r\nDisconnect Response: Interrupt Channel"), 0x80);
 247                                         identifier = l2capinbuf[9];
 248                                         l2cap_set_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE);
 249                                 }
 250                         }
 251 #ifdef EXTRADEBUG
 252                         else {
 253                                 identifier = l2capinbuf[9];
 254                                 Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80);
 255                                 D_PrintHex<uint8_t > (l2capinbuf[8], 0x80);
 256                         }
 257 #endif
 258                 } else if(l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
 259                         //Notify(PSTR("\r\nL2CAP Interrupt"), 0x80);
 260                         if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
 261                                 if((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || (l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons
 262                                         if((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33) // These reports have no extensions bytes
 263                                                 ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
 264                                         else if(wiiUProControllerConnected)
 265                                                 ButtonState = (uint32_t)(((~l2capinbuf[23]) & 0xFE) | ((uint16_t)(~l2capinbuf[24]) << 8) | ((uint32_t)((~l2capinbuf[25]) & 0x03) << 16));
 266                                         else if(motionPlusConnected) {
 267                                                 if(l2capinbuf[20] & 0x02) // Only update the Wiimote buttons, since the extension bytes are from the Motion Plus
 268                                                         ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(ButtonState & 0xFFFF0000)));
 269                                                 else if(nunchuckConnected) // Update if it's a report from the Nunchuck
 270                                                         ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x0C) << 14));
 271                                                 //else if(classicControllerConnected) // Update if it's a report from the Classic Controller
 272                                         } else if(nunchuckConnected) // The Nunchuck is directly connected
 273                                                 ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x03) << 16));
 274                                                 //else if(classicControllerConnected) // The Classic Controller is directly connected
 275                                         else if(!unknownExtensionConnected)
 276                                                 ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
 277 #ifdef PRINTREPORT
 278                                         Notify(PSTR("ButtonState: "), 0x80);
 279                                         D_PrintHex<uint32_t > (ButtonState, 0x80);
 280                                         Notify(PSTR("\r\n"), 0x80);
 281 #endif
 282                                         if(ButtonState != OldButtonState) {
 283                                                 ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable
 284                                                 OldButtonState = ButtonState;
 285                                         }
 286                                 }
 287                                 if(l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33 || l2capinbuf[9] == 0x35 || l2capinbuf[9] == 0x37) { // Read the accelerometer
 288                                         accXwiimote = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5)) - 500;
 289                                         accYwiimote = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4)) - 500;
 290                                         accZwiimote = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5)) - 500;
 291                                 }
 292                                 switch(l2capinbuf[9]) {
 293                                         case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV
 294 #ifdef EXTRADEBUG
 295                                                 Notify(PSTR("\r\nStatus report was received"), 0x80);
 296 #endif
 297                                                 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)
 298                                                 batteryLevel = l2capinbuf[15]; // Update battery level
 299
 300                                                 if(!checkBatteryLevel) { // If this is true it means that the user must have called getBatteryLevel()
 301                                                         if(l2capinbuf[12] & 0x02) { // Check if a extension is connected
 302 #ifdef DEBUG_USB_HOST
 303                                                                 if(!unknownExtensionConnected)
 304                                                                         Notify(PSTR("\r\nExtension connected"), 0x80);
 305 #endif
 306                                                                 unknownExtensionConnected = true;
 307 #ifdef WIICAMERA
 308                                                                 if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
 309 #endif
 310                                                                         setReportMode(false, 0x35); // Also read the extension
 311                                                         } else {
 312 #ifdef DEBUG_USB_HOST
 313                                                                 Notify(PSTR("\r\nExtension disconnected"), 0x80);
 314 #endif
 315                                                                 if(motionPlusConnected) {
 316 #ifdef DEBUG_USB_HOST
 317                                                                         Notify(PSTR(" - from Motion Plus"), 0x80);
 318 #endif
 319                                                                         wii_clear_flag(WII_FLAG_NUNCHUCK_CONNECTED);
 320                                                                         if(!activateNunchuck) // If it's already trying to initialize the Nunchuck don't set it to false
 321                                                                                 nunchuckConnected = false;
 322                                                                         //else if(classicControllerConnected)
 323                                                                 } else if(nunchuckConnected) {
 324 #ifdef DEBUG_USB_HOST
 325                                                                         Notify(PSTR(" - Nunchuck"), 0x80);
 326 #endif
 327                                                                         nunchuckConnected = false; // It must be the Nunchuck controller then
 328                                                                         wii_clear_flag(WII_FLAG_NUNCHUCK_CONNECTED);
 329                                                                         onInit();
 330                                                                         setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer
 331                                                                 } else
 332                                                                         setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer
 333                                                         }
 334                                                 }
 335                                                 else {
 336 #ifdef EXTRADEBUG
 337                                                         Notify(PSTR("\r\nChecking battery level"), 0x80);
 338 #endif
 339                                                         checkBatteryLevel = false; // Check for extensions by default
 340                                                 }
 341 #ifdef DEBUG_USB_HOST
 342                                                 if(l2capinbuf[12] & 0x01)
 343                                                         Notify(PSTR("\r\nWARNING: Battery is nearly empty"), 0x80);
 344 #endif
 345
 346                                                 break;
 347                                         case 0x21: // Read Memory Data
 348                                                 if((l2capinbuf[12] & 0x0F) == 0) { // No error
 349                                                         uint8_t reportLength = (l2capinbuf[12] >> 4) + 1; // // Bit 4-7 is the length - 1
 350                                                         // See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers
 351                                                         if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) {
 352 #ifdef DEBUG_USB_HOST
 353                                                                 Notify(PSTR("\r\nNunchuck connected"), 0x80);
 354 #endif
 355                                                                 wii_set_flag(WII_FLAG_NUNCHUCK_CONNECTED);
 356                                                         } else if(l2capinbuf[16] == 0x00 && (l2capinbuf[17] == 0xA6 || l2capinbuf[17] == 0xA4) && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) {
 357 #ifdef DEBUG_USB_HOST
 358                                                                 Notify(PSTR("\r\nMotion Plus connected"), 0x80);
 359 #endif
 360                                                                 wii_set_flag(WII_FLAG_MOTION_PLUS_CONNECTED);
 361                                                         } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) {
 362 #ifdef DEBUG_USB_HOST
 363                                                                 Notify(PSTR("\r\nMotion Plus activated in normal mode"), 0x80);
 364 #endif
 365                                                                 motionPlusConnected = true;
 366 #ifdef WIICAMERA
 367                                                                 if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
 368 #endif
 369                                                                         setReportMode(false, 0x35); // Also read the extension
 370                                                         } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) {
 371 #ifdef DEBUG_USB_HOST
 372                                                                 Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode"), 0x80);
 373 #endif
 374                                                                 activateNunchuck = false;
 375                                                                 motionPlusConnected = true;
 376                                                                 nunchuckConnected = true;
 377 #ifdef WIICAMERA
 378                                                                 if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
 379 #endif
 380                                                                         setReportMode(false, 0x35); // Also read the extension
 381                                                         } 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) {
 382 #ifdef DEBUG_USB_HOST
 383                                                                 Notify(PSTR("\r\nInactive Wii Motion Plus"), 0x80);
 384                                                                 Notify(PSTR("\r\nPlease unplug the Motion Plus, disconnect the Wiimote and then replug the Motion Plus Extension"), 0x80);
 385 #endif
 386                                                                 stateCounter = 300; // Skip the rest in "WII_CHECK_MOTION_PLUS_STATE"
 387                                                         } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x01 && l2capinbuf[20] == 0x20) {
 388 #ifdef DEBUG_USB_HOST
 389                                                                 Notify(PSTR("\r\nWii U Pro Controller connected"), 0x80);
 390 #endif
 391                                                                 wiiUProControllerConnected = true;
 392                                                         } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x02) {
 393 #ifdef DEBUG_USB_HOST
 394                                                                 Notify(PSTR("\r\nWii Balance Board connected"), 0x80);
 395 #endif
 396                                                                 setReportMode(false, 0x32); // Read the Wii Balance Board extension
 397                                                                 wii_set_flag(WII_FLAG_CALIBRATE_BALANCE_BOARD);
 398                                                         }
 399                                                         // Wii Balance Board calibration reports (24 bits in total)
 400                                                         else if(l2capinbuf[13] == 0x00 && l2capinbuf[14] == 0x24 && reportLength == 16) { // First 16-bit
 401                                                                 for(uint8_t i = 0; i < 2; i++) {
 402                                                                         for(uint8_t j = 0; j < 4; j++)
 403                                                                                 wiiBalanceBoardCal[i][j] = l2capinbuf[16 + 8 * i + 2 * j] | l2capinbuf[15 + 8 * i + 2 * j] << 8;
 404                                                                 }
 405                                                         } else if(l2capinbuf[13] == 0x00 && l2capinbuf[14] == 0x34 && reportLength == 8) { // Last 8-bit
 406                                                                 for(uint8_t j = 0; j < 4; j++)
 407                                                                         wiiBalanceBoardCal[2][j] = l2capinbuf[16 + 2 * j] | l2capinbuf[15 + 2 * j] << 8;
 408 #ifdef DEBUG_USB_HOST
 409                                                                 Notify(PSTR("\r\nWii Balance Board calibration values read successfully"), 0x80);
 410 #endif
 411                                                                 wii_clear_flag(WII_FLAG_CALIBRATE_BALANCE_BOARD);
 412                                                                 wiiBalanceBoardConnected = true;
 413                                                         }
 414 #ifdef DEBUG_USB_HOST
 415                                                         else {
 416                                                                 Notify(PSTR("\r\nUnknown Device: "), 0x80);
 417                                                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
 418                                                                 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
 419                                                                 Notify(PSTR("\r\nData: "), 0x80);
 420                                                                 for(uint8_t i = 0; i < reportLength; i++) {
 421                                                                         D_PrintHex<uint8_t > (l2capinbuf[15 + i], 0x80);
 422                                                                         Notify(PSTR(" "), 0x80);
 423                                                                 }
 424                                                         }
 425 #endif
 426                                                 }
 427 #ifdef EXTRADEBUG
 428                                                 else {
 429                                                         Notify(PSTR("\r\nReport Error: "), 0x80);
 430                                                         D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
 431                                                         D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
 432                                                 }
 433 #endif
 434                                                 break;
 435                                         case 0x22: // Acknowledge output report, return function result
 436 #ifdef DEBUG_USB_HOST
 437                                                 if(l2capinbuf[13] != 0x00) { // Check if there is an error
 438                                                         Notify(PSTR("\r\nCommand failed: "), 0x80);
 439                                                         D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
 440                                                 }
 441 #endif
 442                                                 break;
 443                                         case 0x30: // Core buttons - (a1) 30 BB BB
 444                                                 break;
 445                                         case 0x31: // Core Buttons and Accelerometer - (a1) 31 BB BB AA AA AA
 446                                                 break;
 447                                         case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE
 448                                                 // See: http://wiibrew.org/wiki/Wii_Balance_Board#Data_Format
 449                                                 wiiBalanceBoardRaw[TopRight] = l2capinbuf[13] | l2capinbuf[12] << 8; // Top right
 450                                                 wiiBalanceBoardRaw[BotRight] = l2capinbuf[15] | l2capinbuf[14] << 8; // Bottom right
 451                                                 wiiBalanceBoardRaw[TopLeft] = l2capinbuf[17] | l2capinbuf[16] << 8; // Top left
 452                                                 wiiBalanceBoardRaw[BotLeft] = l2capinbuf[19] | l2capinbuf[18] << 8; // Bottom left
 453                                                 break;
 454                                         case 0x33: // Core Buttons with Accelerometer and 12 IR bytes - (a1) 33 BB BB AA AA AA II II II II II II II II II II II II
 455 #ifdef WIICAMERA
 456                                                 // Read the IR data
 457                                                 IR_object_x1 = (l2capinbuf[15] | ((uint16_t)(l2capinbuf[17] & 0x30) << 4)); // x position
 458                                                 IR_object_y1 = (l2capinbuf[16] | ((uint16_t)(l2capinbuf[17] & 0xC0) << 2)); // y position
 459                                                 IR_object_s1 = (l2capinbuf[17] & 0x0F); // Size value, 0-15
 460
 461                                                 IR_object_x2 = (l2capinbuf[18] | ((uint16_t)(l2capinbuf[20] & 0x30) << 4));
 462                                                 IR_object_y2 = (l2capinbuf[19] | ((uint16_t)(l2capinbuf[20] & 0xC0) << 2));
 463                                                 IR_object_s2 = (l2capinbuf[20] & 0x0F);
 464
 465                                                 IR_object_x3 = (l2capinbuf[21] | ((uint16_t)(l2capinbuf[23] & 0x30) << 4));
 466                                                 IR_object_y3 = (l2capinbuf[22] | ((uint16_t)(l2capinbuf[23] & 0xC0) << 2));
 467                                                 IR_object_s3 = (l2capinbuf[23] & 0x0F);
 468
 469                                                 IR_object_x4 = (l2capinbuf[24] | ((uint16_t)(l2capinbuf[26] & 0x30) << 4));
 470                                                 IR_object_y4 = (l2capinbuf[25] | ((uint16_t)(l2capinbuf[26] & 0xC0) << 2));
 471                                                 IR_object_s4 = (l2capinbuf[26] & 0x0F);
 472 #endif
 473                                                 break;
 474                                         case 0x34: // Core Buttons with 19 Extension bytes - (a1) 34 BB BB EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
 475                                                 break;
 476                                                 /* 0x3e and 0x3f both give unknown report types when report mode is 0x3e or 0x3f with mode number 0x05 */
 477                                         case 0x3E: // Core Buttons with Accelerometer and 32 IR bytes
 478                                                 // (a1) 31 BB BB AA AA AA II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II
 479                                                 // corresponds to output report mode 0x3e
 480
 481                                                 /**** for reading in full mode: DOES NOT WORK YET ****/
 482                                                 /* When it works it will also have intensity and bounding box data */
 483                                                 /*
 484                                                 IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4));
 485                                                 IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2));
 486                                                 IR_object_s1 = (l2capinbuf[15] & 0x0F);
 487                                                  */
 488                                                 break;
 489                                         case 0x3F:
 490                                                 /*
 491                                                 IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4));
 492                                                 IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2));
 493                                                 IR_object_s1 = (l2capinbuf[15] & 0x0F);
 494                                                  */
 495                                                 break;
 496                                         case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes
 497                                                 // (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
 498 #if 1 // Set this to 0 if you don't want to use an extension, this reduceds the size of the library a lot!
 499                                                 if(motionPlusConnected) {
 500                                                         if(l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension
 501                                                                 if(motionValuesReset) { // We will only use the values when the gyro value has been set
 502                                                                         gyroYawRaw = ((l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6)) - gyroYawZero);
 503                                                                         gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)) - gyroRollZero);
 504                                                                         gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)) - gyroPitchZero);
 505
 506                                                                         yawGyroSpeed = (double)gyroYawRaw / ((double)gyroYawZero / yawGyroScale);
 507                                                                         rollGyroSpeed = -(double)gyroRollRaw / ((double)gyroRollZero / rollGyroScale); // We invert these values so they will fit the acc values
 508                                                                         pitchGyroSpeed = (double)gyroPitchRaw / ((double)gyroPitchZero / pitchGyroScale);
 509
 510                                                                         /* The onboard gyro has two ranges for slow and fast mode */
 511                                                                         if(!(l2capinbuf[18] & 0x02)) // Check if fast mode is used
 512                                                                                 yawGyroSpeed *= 4.545;
 513                                                                         if(!(l2capinbuf[18] & 0x01)) // Check if fast mode is used
 514                                                                                 pitchGyroSpeed *= 4.545;
 515                                                                         if(!(l2capinbuf[19] & 0x02)) // Check if fast mode is used
 516                                                                                 rollGyroSpeed *= 4.545;
 517
 518                                                                         compPitch = (0.93 * (compPitch + (pitchGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * getWiimotePitch()); // Use a complimentary filter to calculate the angle
 519                                                                         compRoll = (0.93 * (compRoll + (rollGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * getWiimoteRoll());
 520
 521                                                                         gyroYaw += (yawGyroSpeed * ((double)(micros() - timer) / 1000000));
 522                                                                         gyroRoll += (rollGyroSpeed * ((double)(micros() - timer) / 1000000));
 523                                                                         gyroPitch += (pitchGyroSpeed * ((double)(micros() - timer) / 1000000));
 524                                                                         timer = micros();
 525                                                                         /*
 526                                                                         // Uncomment these lines to tune the gyro scale variabels
 527                                                                         Notify(PSTR("\r\ngyroYaw: "), 0x80);
 528                                                                         Notify(gyroYaw, 0x80);
 529                                                                         Notify(PSTR("\tgyroRoll: "), 0x80);
 530                                                                         Notify(gyroRoll, 0x80);
 531                                                                         Notify(PSTR("\tgyroPitch: "), 0x80);
 532                                                                         Notify(gyroPitch, 0x80);
 533                                                                          */
 534                                                                         /*
 535                                                                         Notify(PSTR("\twiimoteRoll: "), 0x80);
 536                                                                         Notify(wiimoteRoll, 0x80);
 537                                                                         Notify(PSTR("\twiimotePitch: "), 0x80);
 538                                                                         Notify(wiimotePitch, 0x80);
 539                                                                          */
 540                                                                 } else {
 541                                                                         if((micros() - timer) > 1000000) { // Loop for 1 sec before resetting the values
 542 #ifdef DEBUG_USB_HOST
 543                                                                                 Notify(PSTR("\r\nThe gyro values has been reset"), 0x80);
 544 #endif
 545                                                                                 gyroYawZero = (l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6));
 546                                                                                 gyroRollZero = (l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6));
 547                                                                                 gyroPitchZero = (l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6));
 548
 549                                                                                 rollGyroScale = 500; // You might need to adjust these
 550                                                                                 pitchGyroScale = 400;
 551                                                                                 yawGyroScale = 415;
 552
 553                                                                                 gyroYaw = 0;
 554                                                                                 gyroRoll = 0;
 555                                                                                 gyroPitch = 0;
 556
 557                                                                                 motionValuesReset = true;
 558                                                                                 timer = micros();
 559                                                                         }
 560                                                                 }
 561                                                         } else {
 562                                                                 if(nunchuckConnected) {
 563                                                                         hatValues[HatX] = l2capinbuf[15];
 564                                                                         hatValues[HatY] = l2capinbuf[16];
 565                                                                         accXnunchuck = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x10 >> 3)) - 416;
 566                                                                         accYnunchuck = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x20 >> 4)) - 416;
 567                                                                         accZnunchuck = (((l2capinbuf[19] & 0xFE) << 2) | (l2capinbuf[20] & 0xC0 >> 5)) - 416;
 568                                                                 }
 569                                                                 //else if(classicControllerConnected) { }
 570                                                         }
 571                                                         if(l2capinbuf[19] & 0x01) {
 572                                                                 if(!extensionConnected) {
 573                                                                         extensionConnected = true;
 574                                                                         unknownExtensionConnected = true;
 575 #ifdef DEBUG_USB_HOST
 576                                                                         Notify(PSTR("\r\nExtension connected to Motion Plus"), 0x80);
 577 #endif
 578                                                                 }
 579                                                         } else {
 580                                                                 if(extensionConnected && !unknownExtensionConnected) {
 581                                                                         extensionConnected = false;
 582                                                                         unknownExtensionConnected = true;
 583 #ifdef DEBUG_USB_HOST
 584                                                                         Notify(PSTR("\r\nExtension disconnected from Motion Plus"), 0x80);
 585 #endif
 586                                                                         nunchuckConnected = false; // There is no extension connected to the Motion Plus if this report is sent
 587                                                                 }
 588                                                         }
 589
 590                                                 } else if(nunchuckConnected) {
 591                                                         hatValues[HatX] = l2capinbuf[15];
 592                                                         hatValues[HatY] = l2capinbuf[16];
 593                                                         accXnunchuck = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2)) - 416;
 594                                                         accYnunchuck = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4)) - 416;
 595                                                         accZnunchuck = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6)) - 416;
 596                                                 } else if(wiiUProControllerConnected) {
 597                                                         hatValues[LeftHatX] = (l2capinbuf[15] | l2capinbuf[16] << 8);
 598                                                         hatValues[RightHatX] = (l2capinbuf[17] | l2capinbuf[18] << 8);
 599                                                         hatValues[LeftHatY] = (l2capinbuf[19] | l2capinbuf[20] << 8);
 600                                                         hatValues[RightHatY] = (l2capinbuf[21] | l2capinbuf[22] << 8);
 601                                                 }
 602 #endif
 603                                                 break;
 604 #ifdef DEBUG_USB_HOST
 605                                         default:
 606                                                 Notify(PSTR("\r\nUnknown Report type: "), 0x80);
 607                                                 D_PrintHex<uint8_t > (l2capinbuf[9], 0x80);
 608                                                 break;
 609 #endif
 610                                 }
 611                         }
 612                 }
 613                 L2CAP_task();
 614         }
 615 }
 616
 617 void WII::L2CAP_task() {
 618         switch(l2cap_state) {
 619                         /* These states are used if the Wiimote is the host */
 620                 case L2CAP_CONTROL_SUCCESS:
 621                         if(l2cap_check_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)) {
 622 #ifdef DEBUG_USB_HOST
 623                                 Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80);
 624 #endif
 625                                 l2cap_state = L2CAP_INTERRUPT_SETUP;
 626                         }
 627                         break;
 628
 629                 case L2CAP_INTERRUPT_SETUP:
 630                         if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST)) {
 631 #ifdef DEBUG_USB_HOST
 632                                 Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80);
 633 #endif
 634                                 pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, PENDING);
 635                                 delay(1);
 636                                 pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, SUCCESSFUL);
 637                                 identifier++;
 638                                 delay(1);
 639                                 pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid);
 640
 641                                 l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST;
 642                         }
 643                         break;
 644
 645                         /* These states are used if the Arduino is the host */
 646                 case L2CAP_CONTROL_CONNECT_REQUEST:
 647                         if(l2cap_check_flag(L2CAP_FLAG_CONTROL_CONNECTED)) {
 648 #ifdef DEBUG_USB_HOST
 649                                 Notify(PSTR("\r\nSend HID Control Config Request"), 0x80);
 650 #endif
 651                                 identifier++;
 652                                 pBtd->l2cap_config_request(hci_handle, identifier, control_scid);
 653                                 l2cap_state = L2CAP_CONTROL_CONFIG_REQUEST;
 654                         }
 655                         break;
 656
 657                 case L2CAP_CONTROL_CONFIG_REQUEST:
 658                         if(l2cap_check_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)) {
 659 #ifdef DEBUG_USB_HOST
 660                                 Notify(PSTR("\r\nSend HID Interrupt Connection Request"), 0x80);
 661 #endif
 662                                 identifier++;
 663                                 pBtd->l2cap_connection_request(hci_handle, identifier, interrupt_dcid, HID_INTR_PSM);
 664                                 l2cap_state = L2CAP_INTERRUPT_CONNECT_REQUEST;
 665                         }
 666                         break;
 667
 668                 case L2CAP_INTERRUPT_CONNECT_REQUEST:
 669                         if(l2cap_check_flag(L2CAP_FLAG_INTERRUPT_CONNECTED)) {
 670 #ifdef DEBUG_USB_HOST
 671                                 Notify(PSTR("\r\nSend HID Interrupt Config Request"), 0x80);
 672 #endif
 673                                 identifier++;
 674                                 pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid);
 675                                 l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST;
 676                         }
 677                         break;
 678
 679                 case L2CAP_INTERRUPT_CONFIG_REQUEST:
 680                         if(l2cap_check_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS)) { // Now the HID channels is established
 681 #ifdef DEBUG_USB_HOST
 682                                 Notify(PSTR("\r\nHID Channels Established"), 0x80);
 683 #endif
 684                                 pBtd->connectToWii = false;
 685                                 pBtd->pairWithWii = false;
 686                                 stateCounter = 0;
 687                                 l2cap_state = WII_CHECK_MOTION_PLUS_STATE;
 688                         }
 689                         break;
 690
 691                         /* The next states are in run() */
 692
 693                 case L2CAP_INTERRUPT_DISCONNECT:
 694                         if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE) && ((long)(millis() - timer) >= 0L)) {
 695 #ifdef DEBUG_USB_HOST
 696                                 Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80);
 697 #endif
 698                                 identifier++;
 699                                 pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid);
 700                                 l2cap_state = L2CAP_CONTROL_DISCONNECT;
 701                         }
 702                         break;
 703
 704                 case L2CAP_CONTROL_DISCONNECT:
 705                         if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE)) {
 706 #ifdef DEBUG_USB_HOST
 707                                 Notify(PSTR("\r\nDisconnected Control Channel"), 0x80);
 708 #endif
 709                                 pBtd->hci_disconnect(hci_handle);
 710                                 hci_handle = -1; // Reset handle
 711                                 l2cap_event_flag = 0; // Reset flags
 712                                 l2cap_state = L2CAP_WAIT;
 713                         }
 714                         break;
 715         }
 716 }
 717
 718 void WII::Run() {
 719         if(l2cap_state == L2CAP_INTERRUPT_DISCONNECT && ((long)(millis() - timer) >= 0L))
 720                 L2CAP_task(); // Call the rest of the disconnection routine after we have waited long enough
 721
 722         switch(l2cap_state) {
 723                 case L2CAP_WAIT:
 724                         if(pBtd->connectToWii && !pBtd->l2capConnectionClaimed && !wiimoteConnected && !activeConnection) {
 725                                 pBtd->l2capConnectionClaimed = true;
 726                                 activeConnection = true;
 727                                 motionPlusInside = pBtd->motionPlusInside;
 728 #ifdef DEBUG_USB_HOST
 729                                 Notify(PSTR("\r\nSend HID Control Connection Request"), 0x80);
 730 #endif
 731                                 hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
 732                                 l2cap_event_flag = 0; // Reset flags
 733                                 identifier = 0;
 734                                 pBtd->l2cap_connection_request(hci_handle, identifier, control_dcid, HID_CTRL_PSM);
 735                                 l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST;
 736                         } else if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_CONTROL_REQUEST)) {
 737 #ifdef DEBUG_USB_HOST
 738                                 Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80);
 739 #endif
 740                                 pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, PENDING);
 741                                 delay(1);
 742                                 pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, SUCCESSFUL);
 743                                 identifier++;
 744                                 delay(1);
 745                                 pBtd->l2cap_config_request(hci_handle, identifier, control_scid);
 746                                 l2cap_state = L2CAP_CONTROL_SUCCESS;
 747                         }
 748                         break;
 749
 750                 case WII_CHECK_MOTION_PLUS_STATE:
 751 #ifdef DEBUG_USB_HOST
 752                         if(stateCounter == 0) // Only print onnce
 753                                 Notify(PSTR("\r\nChecking if a Motion Plus is connected"), 0x80);
 754 #endif
 755                         stateCounter++;
 756                         if(stateCounter % 200 == 0)
 757                                 checkMotionPresent(); // Check if there is a motion plus connected
 758                         if(wii_check_flag(WII_FLAG_MOTION_PLUS_CONNECTED)) {
 759                                 stateCounter = 0;
 760                                 l2cap_state = WII_INIT_MOTION_PLUS_STATE;
 761                                 timer = micros();
 762
 763                                 if(unknownExtensionConnected) {
 764 #ifdef DEBUG_USB_HOST
 765                                         Notify(PSTR("\r\nA extension is also connected"), 0x80);
 766 #endif
 767                                         activateNunchuck = true; // For we will just set this to true as this the only extension supported so far
 768                                 }
 769
 770                         } else if(stateCounter == 601) { // We will try three times to check for the motion plus
 771 #ifdef DEBUG_USB_HOST
 772                                 Notify(PSTR("\r\nNo Motion Plus was detected"), 0x80);
 773 #endif
 774                                 stateCounter = 0;
 775                                 l2cap_state = WII_CHECK_EXTENSION_STATE;
 776                         }
 777                         break;
 778
 779                 case WII_CHECK_EXTENSION_STATE: // This is used to check if there is anything plugged in to the extension port
 780 #ifdef DEBUG_USB_HOST
 781                         if(stateCounter == 0) // Only print onnce
 782                                 Notify(PSTR("\r\nChecking if there is any extension connected"), 0x80);
 783 #endif
 784                         stateCounter++; // We use this counter as there has to be a short delay between the commands
 785                         if(stateCounter == 1)
 786                                 statusRequest(); // See if a new device has connected
 787                         if(stateCounter == 100) {
 788                                 if(unknownExtensionConnected) // Check if there is a extension is connected to the port
 789                                         initExtension1();
 790                                 else
 791                                         stateCounter = 499;
 792                         } else if(stateCounter == 200)
 793                                 initExtension2();
 794                         else if(stateCounter == 300) {
 795                                 readExtensionType();
 796                                 unknownExtensionConnected = false;
 797                         } else if(stateCounter == 400) {
 798                                 if(wii_check_flag(WII_FLAG_CALIBRATE_BALANCE_BOARD)) {
 799 #ifdef DEBUG_USB_HOST
 800                                         Notify(PSTR("\r\nReading Wii Balance Board calibration values"), 0x80);
 801 #endif
 802                                         readWiiBalanceBoardCalibration();
 803                                 } else
 804                                         stateCounter = 499;
 805                         } else if(stateCounter == 500) {
 806                                 stateCounter = 0;
 807                                 l2cap_state = TURN_ON_LED;
 808                         }
 809                         break;
 810
 811                 case WII_INIT_MOTION_PLUS_STATE:
 812                         stateCounter++;
 813                         if(stateCounter == 1)
 814                                 initMotionPlus();
 815                         else if(stateCounter == 100)
 816                                 activateMotionPlus();
 817                         else if(stateCounter == 200)
 818                                 readExtensionType(); // Check if it has been activated
 819                         else if(stateCounter == 300) {
 820                                 stateCounter = 0;
 821                                 unknownExtensionConnected = false; // The motion plus will send a status report when it's activated, we will set this to false so it doesn't reinitialize the Motion Plus
 822                                 l2cap_state = TURN_ON_LED;
 823                         }
 824                         break;
 825
 826                 case TURN_ON_LED:
 827                         if(wii_check_flag(WII_FLAG_NUNCHUCK_CONNECTED))
 828                                 nunchuckConnected = true;
 829                         wiimoteConnected = true;
 830                         onInit();
 831                         l2cap_state = L2CAP_DONE;
 832                         break;
 833
 834                 case L2CAP_DONE:
 835                         if(unknownExtensionConnected) {
 836 #ifdef DEBUG_USB_HOST
 837                                 if(stateCounter == 0) // Only print once
 838                                         Notify(PSTR("\r\nChecking extension port"), 0x80);
 839 #endif
 840                                 stateCounter++; // We will use this counter as there has to be a short delay between the commands
 841                                 if(stateCounter == 50)
 842                                         statusRequest();
 843                                 else if(stateCounter == 100)
 844                                         initExtension1();
 845                                 else if(stateCounter == 150)
 846                                         if((extensionConnected && motionPlusConnected) || (unknownExtensionConnected && !motionPlusConnected))
 847                                                 initExtension2();
 848                                         else
 849                                                 stateCounter = 299; // There is no extension connected
 850                                 else if(stateCounter == 200)
 851                                         readExtensionType();
 852                                 else if(stateCounter == 250) {
 853                                         if(wii_check_flag(WII_FLAG_NUNCHUCK_CONNECTED)) {
 854 #ifdef DEBUG_USB_HOST
 855                                                 Notify(PSTR("\r\nNunchuck was reconnected"), 0x80);
 856 #endif
 857                                                 activateNunchuck = true;
 858                                                 nunchuckConnected = true;
 859                                         }
 860                                         if(!motionPlusConnected)
 861                                                 stateCounter = 449;
 862                                 } else if(stateCounter == 300) {
 863                                         if(motionPlusConnected) {
 864 #ifdef DEBUG_USB_HOST
 865                                                 Notify(PSTR("\r\nReactivating the Motion Plus"), 0x80);
 866 #endif
 867                                                 initMotionPlus();
 868                                         } else
 869                                                 stateCounter = 449;
 870                                 } else if(stateCounter == 350)
 871                                         activateMotionPlus();
 872                                 else if(stateCounter == 400)
 873                                         readExtensionType(); // Check if it has been activated
 874                                 else if(stateCounter == 450) {
 875                                         onInit();
 876                                         stateCounter = 0;
 877                                         unknownExtensionConnected = false;
 878                                 }
 879                         } else
 880                                 stateCounter = 0;
 881                         break;
 882         }
 883 }
 884
 885 /************************************************************/
 886 /*                    HID Commands                          */
 887 /************************************************************/
 888
 889 void WII::HID_Command(uint8_t* data, uint8_t nbytes) {
 890         if(motionPlusInside)
 891                 pBtd->L2CAP_Command(hci_handle, data, nbytes, interrupt_scid[0], interrupt_scid[1]); // It's the new Wiimote with the Motion Plus Inside or Wii U Pro controller
 892         else
 893                 pBtd->L2CAP_Command(hci_handle, data, nbytes, control_scid[0], control_scid[1]);
 894 }
 895
 896 void WII::setAllOff() {
 897         HIDBuffer[1] = 0x11;
 898         HIDBuffer[2] = 0x00;
 899         HID_Command(HIDBuffer, 3);
 900 }
 901
 902 void WII::setRumbleOff() {
 903         HIDBuffer[1] = 0x11;
 904         HIDBuffer[2] &= ~0x01; // Bit 0 control the rumble
 905         HID_Command(HIDBuffer, 3);
 906 }
 907
 908 void WII::setRumbleOn() {
 909         HIDBuffer[1] = 0x11;
 910         HIDBuffer[2] |= 0x01; // Bit 0 control the rumble
 911         HID_Command(HIDBuffer, 3);
 912 }
 913
 914 void WII::setRumbleToggle() {
 915         HIDBuffer[1] = 0x11;
 916         HIDBuffer[2] ^= 0x01; // Bit 0 control the rumble
 917         HID_Command(HIDBuffer, 3);
 918 }
 919
 920 void WII::setLedRaw(uint8_t value) {
 921         HIDBuffer[1] = 0x11;
 922         HIDBuffer[2] = value | (HIDBuffer[2] & 0x01); // Keep the rumble bit
 923         HID_Command(HIDBuffer, 3);
 924 }
 925
 926 void WII::setLedOff(LEDEnum a) {
 927         HIDBuffer[1] = 0x11;
 928         HIDBuffer[2] &= ~(pgm_read_byte(&WII_LEDS[(uint8_t)a]));
 929         HID_Command(HIDBuffer, 3);
 930 }
 931
 932 void WII::setLedOn(LEDEnum a) {
 933         if(a == OFF)
 934                 setLedRaw(0);
 935         else {
 936                 HIDBuffer[1] = 0x11;
 937                 HIDBuffer[2] |= pgm_read_byte(&WII_LEDS[(uint8_t)a]);
 938                 HID_Command(HIDBuffer, 3);
 939         }
 940 }
 941
 942 void WII::setLedToggle(LEDEnum a) {
 943         HIDBuffer[1] = 0x11;
 944         HIDBuffer[2] ^= pgm_read_byte(&WII_LEDS[(uint8_t)a]);
 945         HID_Command(HIDBuffer, 3);
 946 }
 947
 948 void WII::setLedStatus() {
 949         HIDBuffer[1] = 0x11;
 950         HIDBuffer[2] = (HIDBuffer[2] & 0x01); // Keep the rumble bit
 951         if(wiimoteConnected)
 952                 HIDBuffer[2] |= 0x10; // If it's connected LED1 will light up
 953         if(motionPlusConnected)
 954                 HIDBuffer[2] |= 0x20; // If it's connected LED2 will light up
 955         if(nunchuckConnected)
 956                 HIDBuffer[2] |= 0x40; // If it's connected LED3 will light up
 957
 958         HID_Command(HIDBuffer, 3);
 959 }
 960
 961 uint8_t WII::getBatteryLevel() {
 962         checkBatteryLevel = true; // This is needed so the library knows that the status response is a response to this function
 963         statusRequest(); // This will update the battery level
 964         return batteryLevel;
 965 };
 966
 967 void WII::setReportMode(bool continuous, uint8_t mode) {
 968         uint8_t cmd_buf[4];
 969         cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
 970         cmd_buf[1] = 0x12;
 971         if(continuous)
 972                 cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit
 973         else
 974                 cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Keep the rumble bit
 975         cmd_buf[3] = mode;
 976         HID_Command(cmd_buf, 4);
 977 }
 978
 979 void WII::statusRequest() {
 980         uint8_t cmd_buf[3];
 981         cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
 982         cmd_buf[1] = 0x15;
 983         cmd_buf[2] = (HIDBuffer[2] & 0x01); // Keep the rumble bit
 984         HID_Command(cmd_buf, 3);
 985 }
 986
 987 /************************************************************/
 988 /*                    Memmory Commands                      */
 989 /************************************************************/
 990
 991 void WII::writeData(uint32_t offset, uint8_t size, uint8_t* data) {
 992         uint8_t cmd_buf[23];
 993         cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
 994         cmd_buf[1] = 0x16; // Write data
 995         cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Write to memory, clear bit 2 to write to EEPROM
 996         cmd_buf[3] = (uint8_t)((offset & 0xFF0000) >> 16);
 997         cmd_buf[4] = (uint8_t)((offset & 0xFF00) >> 8);
 998         cmd_buf[5] = (uint8_t)(offset & 0xFF);
 999         cmd_buf[6] = size;
1000         uint8_t i = 0;
1001         for(; i < size; i++)
1002                 cmd_buf[7 + i] = data[i];
1003         for(; i < 16; i++) // Set the rest to zero
1004                 cmd_buf[7 + i] = 0x00;
1005         HID_Command(cmd_buf, 23);
1006 }
1007
1008 void WII::initExtension1() {
1009         uint8_t buf[1];
1010         buf[0] = 0x55;
1011         writeData(0xA400F0, 1, buf);
1012 }
1013
1014 void WII::initExtension2() {
1015         uint8_t buf[1];
1016         buf[0] = 0x00;
1017         writeData(0xA400FB, 1, buf);
1018 }
1019
1020 void WII::initMotionPlus() {
1021         uint8_t buf[1];
1022         buf[0] = 0x55;
1023         writeData(0xA600F0, 1, buf);
1024 }
1025
1026 void WII::activateMotionPlus() {
1027         uint8_t buf[1];
1028         if(pBtd->wiiUProController) {
1029 #ifdef DEBUG_USB_HOST
1030                 Notify(PSTR("\r\nActivating Wii U Pro Controller"), 0x80);
1031 #endif
1032                 buf[0] = 0x00; // It seems like you can send anything but 0x04, 0x05, and 0x07
1033         } else if(activateNunchuck) {
1034 #ifdef DEBUG_USB_HOST
1035                 Notify(PSTR("\r\nActivating Motion Plus in pass-through mode"), 0x80);
1036 #endif
1037                 buf[0] = 0x05; // Activate nunchuck pass-through mode
1038         }//else if(classicControllerConnected && extensionConnected)
1039                 //buf[0] = 0x07;
1040         else {
1041 #ifdef DEBUG_USB_HOST
1042                 Notify(PSTR("\r\nActivating Motion Plus in normal mode"), 0x80);
1043 #endif
1044                 buf[0] = 0x04; // Don't use any extension
1045         }
1046         writeData(0xA600FE, 1, buf);
1047 }
1048
1049 void WII::readData(uint32_t offset, uint16_t size, bool EEPROM) {
1050         uint8_t cmd_buf[8];
1051         cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
1052         cmd_buf[1] = 0x17; // Read data
1053         if(EEPROM)
1054                 cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Read from EEPROM
1055         else
1056                 cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Read from memory
1057         cmd_buf[3] = (uint8_t)((offset & 0xFF0000) >> 16);
1058         cmd_buf[4] = (uint8_t)((offset & 0xFF00) >> 8);
1059         cmd_buf[5] = (uint8_t)(offset & 0xFF);
1060         cmd_buf[6] = (uint8_t)((size & 0xFF00) >> 8);
1061         cmd_buf[7] = (uint8_t)(size & 0xFF);
1062
1063         HID_Command(cmd_buf, 8);
1064 }
1065
1066 void WII::readExtensionType() {
1067         readData(0xA400FA, 6, false);
1068 }
1069
1070 void WII::readCalData() {
1071         readData(0x0016, 8, true);
1072 }
1073
1074 void WII::checkMotionPresent() {
1075         readData(0xA600FA, 6, false);
1076 }
1077
1078 void WII::readWiiBalanceBoardCalibration() {
1079         readData(0xA40024, 24, false);
1080 }
1081
1082 /************************************************************/
1083 /*                    WII Commands                          */
1084 /************************************************************/
1085
1086 bool WII::getButtonPress(ButtonEnum b) { // Return true when a button is pressed
1087         if(wiiUProControllerConnected)
1088                 return (ButtonState & pgm_read_dword(&WII_PROCONTROLLER_BUTTONS[(uint8_t)b]));
1089         else
1090                 return (ButtonState & pgm_read_dword(&WII_BUTTONS[(uint8_t)b]));
1091 }
1092
1093 bool WII::getButtonClick(ButtonEnum b) { // Only return true when a button is clicked
1094         uint32_t button;
1095         if(wiiUProControllerConnected)
1096                 button = pgm_read_dword(&WII_PROCONTROLLER_BUTTONS[(uint8_t)b]);
1097         else
1098                 button = pgm_read_dword(&WII_BUTTONS[(uint8_t)b]);
1099         bool click = (ButtonClickState & button);
1100         ButtonClickState &= ~button; // clear "click" event
1101         return click;
1102 }
1103
1104 uint8_t WII::getAnalogHat(HatEnum a) {
1105         if(!nunchuckConnected)
1106                 return 127; // Return center position
1107         else {
1108                 uint8_t output = hatValues[(uint8_t)a];
1109                 if(output == 0xFF || output == 0x00) // The joystick will only read 255 or 0 when the cable is unplugged or initializing, so we will just return the center position
1110                         return 127;
1111                 else
1112                         return output;
1113         }
1114 }
1115
1116 uint16_t WII::getAnalogHat(AnalogHatEnum a) {
1117         if(!wiiUProControllerConnected)
1118                 return 2000;
1119         else {
1120                 uint16_t output = hatValues[(uint8_t)a];
1121                 if(output == 0x00) // The joystick will only read 0 when it is first initializing, so we will just return the center position
1122                         return 2000;
1123                 else
1124                         return output;
1125         }
1126 }
1127
1128 void WII::onInit() {
1129         if(pFuncOnInit)
1130                 pFuncOnInit(); // Call the user function
1131         else
1132                 setLedStatus();
1133 }
1134
1135 /************************************************************/
1136 /*                 Wii Balance Board Commands               */
1137 /************************************************************/
1138
1139 float WII::getWeight(BalanceBoardEnum pos) {
1140         // Use interpolating between two points - based on: https://github.com/skorokithakis/gr8w8upd8m8/blob/master/gr8w8upd8m8.py
1141         // wiiBalanceBoardCal[pos][0] is calibration values for 0 kg
1142         // wiiBalanceBoardCal[pos][1] is calibration values for 17 kg
1143         // wiiBalanceBoardCal[pos][2] is calibration values for 34 kg
1144         if(wiiBalanceBoardRaw[pos] < wiiBalanceBoardCal[0][pos])
1145             return 0.0f; // Below 0 kg
1146         else if(wiiBalanceBoardRaw[pos] < wiiBalanceBoardCal[1][pos]) // Between 0 and 17 kg
1147             return 17.0f * (float)(wiiBalanceBoardRaw[pos] - wiiBalanceBoardCal[0][pos]) / (float)(wiiBalanceBoardCal[1][pos] - wiiBalanceBoardCal[0][pos]);
1148         else // More than 17 kg
1149             return 17.0f + 17.0f * (float)(wiiBalanceBoardRaw[pos] - wiiBalanceBoardCal[1][pos]) / (float)(wiiBalanceBoardCal[2][pos] - wiiBalanceBoardCal[1][pos]);
1150 };
1151
1152 float WII::getTotalWeight() {
1153         return getWeight(TopRight) + getWeight(BotRight) + getWeight(TopLeft) + getWeight(BotLeft);
1154 };
1155
1156 /************************************************************/
1157 /*       The following functions are for the IR camera      */
1158 /************************************************************/
1159
1160 #ifdef WIICAMERA
1161
1162 void WII::IRinitialize() { // Turns on and initialises the IR camera
1163
1164         enableIRCamera1();
1165 #ifdef DEBUG_USB_HOST
1166         Notify(PSTR("\r\nEnable IR Camera1 Complete"), 0x80);
1167 #endif
1168         delay(80);
1169
1170         enableIRCamera2();
1171 #ifdef DEBUG_USB_HOST
1172         Notify(PSTR("\r\nEnable IR Camera2 Complete"), 0x80);
1173 #endif
1174         delay(80);
1175
1176         write0x08Value();
1177 #ifdef DEBUG_USB_HOST
1178         Notify(PSTR("\r\nWrote hex number 0x08"), 0x80);
1179 #endif
1180         delay(80);
1181
1182         writeSensitivityBlock1();
1183 #ifdef DEBUG_USB_HOST
1184         Notify(PSTR("\r\nWrote Sensitivity Block 1"), 0x80);
1185 #endif
1186         delay(80);
1187
1188         writeSensitivityBlock2();
1189 #ifdef DEBUG_USB_HOST
1190         Notify(PSTR("\r\nWrote Sensitivity Block 2"), 0x80);
1191 #endif
1192         delay(80);
1193
1194         uint8_t mode_num = 0x03;
1195         setWiiModeNumber(mode_num); // Change input for whatever mode you want i.e. 0x01, 0x03, or 0x05
1196 #ifdef DEBUG_USB_HOST
1197         Notify(PSTR("\r\nSet Wii Mode Number To 0x"), 0x80);
1198         D_PrintHex<uint8_t > (mode_num, 0x80);
1199 #endif
1200         delay(80);
1201
1202         write0x08Value();
1203 #ifdef DEBUG_USB_HOST
1204         Notify(PSTR("\r\nWrote Hex Number 0x08"), 0x80);
1205 #endif
1206         delay(80);
1207
1208         setReportMode(false, 0x33);
1209         //setReportMode(false, 0x3f); // For full reporting mode, doesn't work yet
1210 #ifdef DEBUG_USB_HOST
1211         Notify(PSTR("\r\nSet Report Mode to 0x33"), 0x80);
1212 #endif
1213         delay(80);
1214
1215         statusRequest(); // Used to update wiiState - call isIRCameraEnabled() afterwards to check if it actually worked
1216 #ifdef DEBUG_USB_HOST
1217         Notify(PSTR("\r\nIR Initialized"), 0x80);
1218 #endif
1219 }
1220
1221 void WII::enableIRCamera1() {
1222         uint8_t cmd_buf[3];
1223         cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
1224         cmd_buf[1] = 0x13; // Output report 13
1225         cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2
1226         HID_Command(cmd_buf, 3);
1227 }
1228
1229 void WII::enableIRCamera2() {
1230         uint8_t cmd_buf[3];
1231         cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
1232         cmd_buf[1] = 0x1A; // Output report 1A
1233         cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2
1234         HID_Command(cmd_buf, 3);
1235 }
1236
1237 void WII::writeSensitivityBlock1() {
1238         uint8_t buf[9];
1239         buf[0] = 0x00;
1240         buf[1] = 0x00;
1241         buf[2] = 0x00;
1242         buf[3] = 0x00;
1243         buf[4] = 0x00;
1244         buf[5] = 0x00;
1245         buf[6] = 0x90;
1246         buf[7] = 0x00;
1247         buf[8] = 0x41;
1248
1249         writeData(0xB00000, 9, buf);
1250 }
1251
1252 void WII::writeSensitivityBlock2() {
1253         uint8_t buf[2];
1254         buf[0] = 0x40;
1255         buf[1] = 0x00;
1256
1257         writeData(0xB0001A, 2, buf);
1258 }
1259
1260 void WII::write0x08Value() {
1261         uint8_t cmd = 0x08;
1262         writeData(0xb00030, 1, &cmd);
1263 }
1264
1265 void WII::setWiiModeNumber(uint8_t mode_number) { // mode_number in hex i.e. 0x03 for extended mode
1266         writeData(0xb00033, 1, &mode_number);
1267 }
1268 #endif