protocol/usb_hid/USB_Host_Shield_2.0/PS3BT.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
  18 #include "PS3BT.h"
  19 // To enable serial debugging see "settings.h"
  20 //#define EXTRADEBUG // Uncomment to get even more debugging data
  21 //#define PRINTREPORT // Uncomment to print the report send by the PS3 Controllers
  22
  23 PS3BT::PS3BT(BTD *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0) :
  24 BluetoothService(p) // Pointer to USB class instance - mandatory
  25 {
  26         pBtd->my_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead
  27         pBtd->my_bdaddr[4] = btadr4;
  28         pBtd->my_bdaddr[3] = btadr3;
  29         pBtd->my_bdaddr[2] = btadr2;
  30         pBtd->my_bdaddr[1] = btadr1;
  31         pBtd->my_bdaddr[0] = btadr0;
  32
  33         HIDBuffer[0] = 0x52; // HID BT Set_report (0x50) | Report Type (Output 0x02)
  34         HIDBuffer[1] = 0x01; // Report ID
  35
  36         // Needed for PS3 Move Controller commands to work via bluetooth
  37         HIDMoveBuffer[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
  38         HIDMoveBuffer[1] = 0x02; // Report ID
  39
  40         /* Set device cid for the control and intterrupt channelse - LSB */
  41         control_dcid[0] = 0x40; // 0x0040
  42         control_dcid[1] = 0x00;
  43         interrupt_dcid[0] = 0x41; // 0x0041
  44         interrupt_dcid[1] = 0x00;
  45
  46         Reset();
  47 }
  48
  49 bool PS3BT::getButtonPress(ButtonEnum b) {
  50         return (ButtonState & pgm_read_dword(&PS3_BUTTONS[(uint8_t)b]));
  51 }
  52
  53 bool PS3BT::getButtonClick(ButtonEnum b) {
  54         uint32_t button = pgm_read_dword(&PS3_BUTTONS[(uint8_t)b]);
  55         bool click = (ButtonClickState & button);
  56         ButtonClickState &= ~button; // Clear "click" event
  57         return click;
  58 }
  59
  60 uint8_t PS3BT::getAnalogButton(ButtonEnum a) {
  61         return (uint8_t)(l2capinbuf[pgm_read_byte(&PS3_ANALOG_BUTTONS[(uint8_t)a])]);
  62 }
  63
  64 uint8_t PS3BT::getAnalogHat(AnalogHatEnum a) {
  65         return (uint8_t)(l2capinbuf[(uint8_t)a + 15]);
  66 }
  67
  68 int16_t PS3BT::getSensor(SensorEnum a) {
  69         if(PS3Connected) {
  70                 if(a == aX || a == aY || a == aZ || a == gZ)
  71                         return ((l2capinbuf[(uint16_t)a] << 8) | l2capinbuf[(uint16_t)a + 1]);
  72                 else
  73                         return 0;
  74         } else if(PS3MoveConnected) {
  75                 if(a == mXmove || a == mYmove) // These are all 12-bits long
  76                         return (((l2capinbuf[(uint16_t)a] & 0x0F) << 8) | (l2capinbuf[(uint16_t)a + 1]));
  77                 else if(a == mZmove || a == tempMove) // The tempearature is also 12 bits long
  78                         return ((l2capinbuf[(uint16_t)a] << 4) | ((l2capinbuf[(uint16_t)a + 1] & 0xF0) >> 4));
  79                 else // aXmove, aYmove, aZmove, gXmove, gYmove and gZmove
  80                         return (l2capinbuf[(uint16_t)a] | (l2capinbuf[(uint16_t)a + 1] << 8));
  81         } else
  82                 return 0;
  83 }
  84
  85 double PS3BT::getAngle(AngleEnum a) {
  86         double accXval, accYval, accZval;
  87
  88         if(PS3Connected) {
  89                 // Data for the Kionix KXPC4 used in the DualShock 3
  90                 const double zeroG = 511.5; // 1.65/3.3*1023 (1.65V)
  91                 accXval = -((double)getSensor(aX) - zeroG);
  92                 accYval = -((double)getSensor(aY) - zeroG);
  93                 accZval = -((double)getSensor(aZ) - zeroG);
  94         } else if(PS3MoveConnected) {
  95                 // It's a Kionix KXSC4 inside the Motion controller
  96                 const uint16_t zeroG = 0x8000;
  97                 accXval = -(int16_t)(getSensor(aXmove) - zeroG);
  98                 accYval = (int16_t)(getSensor(aYmove) - zeroG);
  99                 accZval = (int16_t)(getSensor(aZmove) - zeroG);
 100         } else
 101                 return 0;
 102
 103         // Convert to 360 degrees resolution
 104         // atan2 outputs the value of -π to π (radians)
 105         // We are then converting it to 0 to 2π and then to degrees
 106         if(a == Pitch)
 107                 return (atan2(accYval, accZval) + PI) * RAD_TO_DEG;
 108         else
 109                 return (atan2(accXval, accZval) + PI) * RAD_TO_DEG;
 110 }
 111
 112 double PS3BT::get9DOFValues(SensorEnum a) { // Thanks to Manfred Piendl
 113         if(!PS3MoveConnected)
 114                 return 0;
 115         int16_t value = getSensor(a);
 116         if(a == mXmove || a == mYmove || a == mZmove) {
 117                 if(value > 2047)
 118                         value -= 0x1000;
 119                 return (double)value / 3.2; // unit: muT = 10^(-6) Tesla
 120         } else if(a == aXmove || a == aYmove || a == aZmove) {
 121                 if(value < 0)
 122                         value += 0x8000;
 123                 else
 124                         value -= 0x8000;
 125                 return (double)value / 442.0; // unit: m/(s^2)
 126         } else if(a == gXmove || a == gYmove || a == gZmove) {
 127                 if(value < 0)
 128                         value += 0x8000;
 129                 else
 130                         value -= 0x8000;
 131                 if(a == gXmove)
 132                         return (double)value / 11.6; // unit: deg/s
 133                 else if(a == gYmove)
 134                         return (double)value / 11.2; // unit: deg/s
 135                 else // gZmove
 136                         return (double)value / 9.6; // unit: deg/s
 137         } else
 138                 return 0;
 139 }
 140
 141 String PS3BT::getTemperature() {
 142         if(PS3MoveConnected) {
 143                 int16_t input = getSensor(tempMove);
 144
 145                 String output = String(input / 100);
 146                 output += ".";
 147                 if(input % 100 < 10)
 148                         output += "0";
 149                 output += String(input % 100);
 150
 151                 return output;
 152         } else
 153                 return "Error";
 154 }
 155
 156 bool PS3BT::getStatus(StatusEnum c) {
 157         return (l2capinbuf[(uint16_t)c >> 8] == ((uint8_t)c & 0xff));
 158 }
 159
 160 void PS3BT::printStatusString() {
 161         char statusOutput[100]; // Max string length plus null character
 162         if(PS3Connected || PS3NavigationConnected) {
 163                 strcpy_P(statusOutput, PSTR("ConnectionStatus: "));
 164
 165                 if(getStatus(Plugged)) strcat_P(statusOutput, PSTR("Plugged"));
 166                 else if(getStatus(Unplugged)) strcat_P(statusOutput, PSTR("Unplugged"));
 167                 else strcat_P(statusOutput, PSTR("Error"));
 168
 169                 strcat_P(statusOutput, PSTR(" - PowerRating: "));
 170
 171                 if(getStatus(Charging)) strcat_P(statusOutput, PSTR("Charging"));
 172                 else if(getStatus(NotCharging)) strcat_P(statusOutput, PSTR("Not Charging"));
 173                 else if(getStatus(Shutdown)) strcat_P(statusOutput, PSTR("Shutdown"));
 174                 else if(getStatus(Dying)) strcat_P(statusOutput, PSTR("Dying"));
 175                 else if(getStatus(Low)) strcat_P(statusOutput, PSTR("Low"));
 176                 else if(getStatus(High)) strcat_P(statusOutput, PSTR("High"));
 177                 else if(getStatus(Full)) strcat_P(statusOutput, PSTR("Full"));
 178                 else strcat_P(statusOutput, PSTR("Error"));
 179
 180                 strcat_P(statusOutput, PSTR(" - WirelessStatus: "));
 181
 182                 if(getStatus(CableRumble)) strcat_P(statusOutput, PSTR("Cable - Rumble is on"));
 183                 else if(getStatus(Cable)) strcat_P(statusOutput, PSTR("Cable - Rumble is off"));
 184                 else if(getStatus(BluetoothRumble)) strcat_P(statusOutput, PSTR("Bluetooth - Rumble is on"));
 185                 else if(getStatus(Bluetooth)) strcat_P(statusOutput, PSTR("Bluetooth - Rumble is off"));
 186                 else strcat_P(statusOutput, PSTR("Error"));
 187         } else if(PS3MoveConnected) {
 188                 strcpy_P(statusOutput, PSTR("PowerRating: "));
 189
 190                 if(getStatus(MoveCharging)) strcat_P(statusOutput, PSTR("Charging"));
 191                 else if(getStatus(MoveNotCharging)) strcat_P(statusOutput, PSTR("Not Charging"));
 192                 else if(getStatus(MoveShutdown)) strcat_P(statusOutput, PSTR("Shutdown"));
 193                 else if(getStatus(MoveDying)) strcat_P(statusOutput, PSTR("Dying"));
 194                 else if(getStatus(MoveLow)) strcat_P(statusOutput, PSTR("Low"));
 195                 else if(getStatus(MoveHigh)) strcat_P(statusOutput, PSTR("High"));
 196                 else if(getStatus(MoveFull)) strcat_P(statusOutput, PSTR("Full"));
 197                 else strcat_P(statusOutput, PSTR("Error"));
 198         } else
 199                 strcpy_P(statusOutput, PSTR("Error"));
 200
 201         USB_HOST_SERIAL.write(statusOutput);
 202 }
 203
 204 void PS3BT::Reset() {
 205         PS3Connected = false;
 206         PS3MoveConnected = false;
 207         PS3NavigationConnected = false;
 208         activeConnection = false;
 209         l2cap_event_flag = 0; // Reset flags
 210         l2cap_state = L2CAP_WAIT;
 211
 212         // Needed for PS3 Dualshock Controller commands to work via Bluetooth
 213         for(uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++)
 214                 HIDBuffer[i + 2] = pgm_read_byte(&PS3_REPORT_BUFFER[i]); // First two bytes reserved for report type and ID
 215 }
 216
 217 void PS3BT::disconnect() { // Use this void to disconnect any of the controllers
 218         // First the HID interrupt channel has to be disconnected, then the HID control channel and finally the HCI connection
 219         pBtd->l2cap_disconnection_request(hci_handle, ++identifier, interrupt_scid, interrupt_dcid);
 220         Reset();
 221         l2cap_state = L2CAP_INTERRUPT_DISCONNECT;
 222 }
 223
 224 void PS3BT::ACLData(uint8_t* ACLData) {
 225         if(!pBtd->l2capConnectionClaimed && !PS3Connected && !PS3MoveConnected && !PS3NavigationConnected && !activeConnection && !pBtd->connectToWii && !pBtd->incomingWii && !pBtd->pairWithWii) {
 226                 if(ACLData[8] == L2CAP_CMD_CONNECTION_REQUEST) {
 227                         if((ACLData[12] | (ACLData[13] << 8)) == HID_CTRL_PSM) {
 228                                 pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service
 229                                 activeConnection = true;
 230                                 hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
 231                                 l2cap_state = L2CAP_WAIT;
 232                                 remote_name_first = pBtd->remote_name[0]; // Store the first letter in remote name for the connection
 233 #ifdef DEBUG_USB_HOST
 234                                 if(pBtd->hci_version < 3) { // Check the HCI Version of the Bluetooth dongle
 235                                         Notify(PSTR("\r\nYour dongle may not support reading the analog buttons, sensors and status\r\nYour HCI Version is: "), 0x80);
 236                                         Notify(pBtd->hci_version, 0x80);
 237                                         Notify(PSTR("\r\nBut should be at least 3\r\nThis means that it doesn't support Bluetooth Version 2.0+EDR"), 0x80);
 238                                 }
 239 #endif
 240                         }
 241                 }
 242         }
 243
 244         if(checkHciHandle(ACLData, hci_handle)) { // acl_handle_ok
 245                 memcpy(l2capinbuf, ACLData, BULK_MAXPKTSIZE);
 246                 if((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001U) { // l2cap_control - Channel ID for ACL-U
 247                         if(l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
 248 #ifdef DEBUG_USB_HOST
 249                                 Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80);
 250                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
 251                                 Notify(PSTR(" "), 0x80);
 252                                 D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
 253                                 Notify(PSTR(" Data: "), 0x80);
 254                                 D_PrintHex<uint8_t > (l2capinbuf[17], 0x80);
 255                                 Notify(PSTR(" "), 0x80);
 256                                 D_PrintHex<uint8_t > (l2capinbuf[16], 0x80);
 257                                 Notify(PSTR(" "), 0x80);
 258                                 D_PrintHex<uint8_t > (l2capinbuf[15], 0x80);
 259                                 Notify(PSTR(" "), 0x80);
 260                                 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
 261 #endif
 262                         } else if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
 263 #ifdef EXTRADEBUG
 264                                 Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80);
 265                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
 266                                 Notify(PSTR(" "), 0x80);
 267                                 D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
 268                                 Notify(PSTR(" SCID: "), 0x80);
 269                                 D_PrintHex<uint8_t > (l2capinbuf[15], 0x80);
 270                                 Notify(PSTR(" "), 0x80);
 271                                 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
 272                                 Notify(PSTR(" Identifier: "), 0x80);
 273                                 D_PrintHex<uint8_t > (l2capinbuf[9], 0x80);
 274 #endif
 275                                 if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
 276                                         identifier = l2capinbuf[9];
 277                                         control_scid[0] = l2capinbuf[14];
 278                                         control_scid[1] = l2capinbuf[15];
 279                                         l2cap_set_flag(L2CAP_FLAG_CONNECTION_CONTROL_REQUEST);
 280                                 } else if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) {
 281                                         identifier = l2capinbuf[9];
 282                                         interrupt_scid[0] = l2capinbuf[14];
 283                                         interrupt_scid[1] = l2capinbuf[15];
 284                                         l2cap_set_flag(L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST);
 285                                 }
 286                         } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
 287                                 if((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
 288                                         if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
 289                                                 //Notify(PSTR("\r\nHID Control Configuration Complete"), 0x80);
 290                                                 l2cap_set_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS);
 291                                         } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
 292                                                 //Notify(PSTR("\r\nHID Interrupt Configuration Complete"), 0x80);
 293                                                 l2cap_set_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS);
 294                                         }
 295                                 }
 296                         } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
 297                                 if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
 298                                         //Notify(PSTR("\r\nHID Control Configuration Request"), 0x80);
 299                                         pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], control_scid);
 300                                 } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
 301                                         //Notify(PSTR("\r\nHID Interrupt Configuration Request"), 0x80);
 302                                         pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], interrupt_scid);
 303                                 }
 304                         } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
 305                                 if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
 306 #ifdef DEBUG_USB_HOST
 307                                         Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80);
 308 #endif
 309                                         identifier = l2capinbuf[9];
 310                                         pBtd->l2cap_disconnection_response(hci_handle, identifier, control_dcid, control_scid);
 311                                         Reset();
 312                                 } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
 313 #ifdef DEBUG_USB_HOST
 314                                         Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80);
 315 #endif
 316                                         identifier = l2capinbuf[9];
 317                                         pBtd->l2cap_disconnection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid);
 318                                         Reset();
 319                                 }
 320                         } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
 321                                 if(l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
 322                                         //Notify(PSTR("\r\nDisconnect Response: Control Channel"), 0x80);
 323                                         identifier = l2capinbuf[9];
 324                                         l2cap_set_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE);
 325                                 } else if(l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) {
 326                                         //Notify(PSTR("\r\nDisconnect Response: Interrupt Channel"), 0x80);
 327                                         identifier = l2capinbuf[9];
 328                                         l2cap_set_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE);
 329                                 }
 330                         }
 331 #ifdef EXTRADEBUG
 332                         else {
 333                                 Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80);
 334                                 D_PrintHex<uint8_t > (l2capinbuf[8], 0x80);
 335                         }
 336 #endif
 337                 } else if(l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
 338                         //Notify(PSTR("\r\nL2CAP Interrupt"), 0x80);
 339                         if(PS3Connected || PS3MoveConnected || PS3NavigationConnected) {
 340                                 /* Read Report */
 341                                 if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
 342                                         lastMessageTime = millis(); // Store the last message time
 343
 344                                         if(PS3Connected || PS3NavigationConnected)
 345                                                 ButtonState = (uint32_t)(l2capinbuf[11] | ((uint16_t)l2capinbuf[12] << 8) | ((uint32_t)l2capinbuf[13] << 16));
 346                                         else if(PS3MoveConnected)
 347                                                 ButtonState = (uint32_t)(l2capinbuf[10] | ((uint16_t)l2capinbuf[11] << 8) | ((uint32_t)l2capinbuf[12] << 16));
 348
 349                                         //Notify(PSTR("\r\nButtonState", 0x80);
 350                                         //PrintHex<uint32_t>(ButtonState, 0x80);
 351
 352                                         if(ButtonState != OldButtonState) {
 353                                                 ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable
 354                                                 OldButtonState = ButtonState;
 355                                         }
 356
 357 #ifdef PRINTREPORT // Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers
 358                                         for(uint8_t i = 10; i < 58; i++) {
 359                                                 D_PrintHex<uint8_t > (l2capinbuf[i], 0x80);
 360                                                 Notify(PSTR(" "), 0x80);
 361                                         }
 362                                         Notify(PSTR("\r\n"), 0x80);
 363 #endif
 364                                 }
 365                         }
 366                 }
 367                 L2CAP_task();
 368         }
 369 }
 370
 371 void PS3BT::L2CAP_task() {
 372         switch(l2cap_state) {
 373                 case L2CAP_WAIT:
 374                         if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_CONTROL_REQUEST)) {
 375 #ifdef DEBUG_USB_HOST
 376                                 Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80);
 377 #endif
 378                                 pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, PENDING);
 379                                 delay(1);
 380                                 pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, SUCCESSFUL);
 381                                 identifier++;
 382                                 delay(1);
 383                                 pBtd->l2cap_config_request(hci_handle, identifier, control_scid);
 384                                 l2cap_state = L2CAP_CONTROL_SUCCESS;
 385                         }
 386                         break;
 387
 388                 case L2CAP_CONTROL_SUCCESS:
 389                         if(l2cap_check_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)) {
 390 #ifdef DEBUG_USB_HOST
 391                                 Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80);
 392 #endif
 393                                 l2cap_state = L2CAP_INTERRUPT_SETUP;
 394                         }
 395                         break;
 396
 397                 case L2CAP_INTERRUPT_SETUP:
 398                         if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST)) {
 399 #ifdef DEBUG_USB_HOST
 400                                 Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80);
 401 #endif
 402                                 pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, PENDING);
 403                                 delay(1);
 404                                 pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, SUCCESSFUL);
 405                                 identifier++;
 406                                 delay(1);
 407                                 pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid);
 408
 409                                 l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST;
 410                         }
 411                         break;
 412
 413                 case L2CAP_INTERRUPT_CONFIG_REQUEST:
 414                         if(l2cap_check_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS)) { // Now the HID channels is established
 415 #ifdef DEBUG_USB_HOST
 416                                 Notify(PSTR("\r\nHID Interrupt Successfully Configured"), 0x80);
 417 #endif
 418                                 if(remote_name_first == 'M') { // First letter in Motion Controller ('M')
 419                                         memset(l2capinbuf, 0, BULK_MAXPKTSIZE); // Reset l2cap in buffer as it sometimes read it as a button has been pressed
 420                                         l2cap_state = TURN_ON_LED;
 421                                 } else
 422                                         l2cap_state = PS3_ENABLE_SIXAXIS;
 423                                 timer = millis();
 424                         }
 425                         break;
 426
 427                         /* These states are handled in Run() */
 428
 429                 case L2CAP_INTERRUPT_DISCONNECT:
 430                         if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE)) {
 431 #ifdef DEBUG_USB_HOST
 432                                 Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80);
 433 #endif
 434                                 identifier++;
 435                                 pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid);
 436                                 l2cap_state = L2CAP_CONTROL_DISCONNECT;
 437                         }
 438                         break;
 439
 440                 case L2CAP_CONTROL_DISCONNECT:
 441                         if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE)) {
 442 #ifdef DEBUG_USB_HOST
 443                                 Notify(PSTR("\r\nDisconnected Control Channel"), 0x80);
 444 #endif
 445                                 pBtd->hci_disconnect(hci_handle);
 446                                 hci_handle = -1; // Reset handle
 447                                 l2cap_event_flag = 0; // Reset flags
 448                                 l2cap_state = L2CAP_WAIT;
 449                         }
 450                         break;
 451         }
 452 }
 453
 454 void PS3BT::Run() {
 455         switch(l2cap_state) {
 456                 case PS3_ENABLE_SIXAXIS:
 457                         if(millis() - timer > 1000) { // loop 1 second before sending the command
 458                                 memset(l2capinbuf, 0, BULK_MAXPKTSIZE); // Reset l2cap in buffer as it sometimes read it as a button has been pressed
 459                                 for(uint8_t i = 15; i < 19; i++)
 460                                         l2capinbuf[i] = 0x7F; // Set the analog joystick values to center position
 461                                 enable_sixaxis();
 462                                 l2cap_state = TURN_ON_LED;
 463                                 timer = millis();
 464                         }
 465                         break;
 466
 467                 case TURN_ON_LED:
 468                         if(millis() - timer > 1000) { // loop 1 second before sending the command
 469                                 if(remote_name_first == 'P') { // First letter in PLAYSTATION(R)3 Controller ('P')
 470 #ifdef DEBUG_USB_HOST
 471                                         Notify(PSTR("\r\nDualshock 3 Controller Enabled\r\n"), 0x80);
 472 #endif
 473                                         PS3Connected = true;
 474                                 } else if(remote_name_first == 'N') { // First letter in Navigation Controller ('N')
 475 #ifdef DEBUG_USB_HOST
 476                                         Notify(PSTR("\r\nNavigation Controller Enabled\r\n"), 0x80);
 477 #endif
 478                                         PS3NavigationConnected = true;
 479                                 } else if(remote_name_first == 'M') { // First letter in Motion Controller ('M')
 480                                         timer = millis();
 481 #ifdef DEBUG_USB_HOST
 482                                         Notify(PSTR("\r\nMotion Controller Enabled\r\n"), 0x80);
 483 #endif
 484                                         PS3MoveConnected = true;
 485                                 }
 486                                 ButtonState = 0; // Clear all values
 487                                 OldButtonState = 0;
 488                                 ButtonClickState = 0;
 489
 490                                 onInit(); // Turn on the LED on the controller
 491                                 l2cap_state = L2CAP_DONE;
 492                         }
 493                         break;
 494
 495                 case L2CAP_DONE:
 496                         if(PS3MoveConnected) { // The Bulb and rumble values, has to be send at approximately every 5th second for it to stay on
 497                                 if(millis() - timer > 4000) { // Send at least every 4th second
 498                                         HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE); // The Bulb and rumble values, has to be written again and again, for it to stay turned on
 499                                         timer = millis();
 500                                 }
 501                         }
 502                         break;
 503         }
 504 }
 505
 506 /************************************************************/
 507 /*                    HID Commands                          */
 508 /************************************************************/
 509
 510 // Playstation Sixaxis Dualshock and Navigation Controller commands
 511
 512 void PS3BT::HID_Command(uint8_t* data, uint8_t nbytes) {
 513         if(millis() - timerHID <= 150) // Check if is has been more than 150ms since last command
 514                 delay((uint32_t)(150 - (millis() - timerHID))); // There have to be a delay between commands
 515         pBtd->L2CAP_Command(hci_handle, data, nbytes, control_scid[0], control_scid[1]); // Both the Navigation and Dualshock controller sends data via the control channel
 516         timerHID = millis();
 517 }
 518
 519 void PS3BT::setAllOff() {
 520         HIDBuffer[3] = 0x00; // Rumble bytes
 521         HIDBuffer[4] = 0x00;
 522         HIDBuffer[5] = 0x00;
 523         HIDBuffer[6] = 0x00;
 524
 525         HIDBuffer[11] = 0x00; // LED byte
 526
 527         HID_Command(HIDBuffer, HID_BUFFERSIZE);
 528 }
 529
 530 void PS3BT::setRumbleOff() {
 531         HIDBuffer[3] = 0x00;
 532         HIDBuffer[4] = 0x00;
 533         HIDBuffer[5] = 0x00;
 534         HIDBuffer[6] = 0x00;
 535
 536         HID_Command(HIDBuffer, HID_BUFFERSIZE);
 537 }
 538
 539 void PS3BT::setRumbleOn(RumbleEnum mode) {
 540         uint8_t power[2] = {0xff, 0x00}; // Defaults to RumbleLow
 541         if(mode == RumbleHigh) {
 542                 power[0] = 0x00;
 543                 power[1] = 0xff;
 544         }
 545         setRumbleOn(0xfe, power[0], 0xfe, power[1]);
 546 }
 547
 548 void PS3BT::setRumbleOn(uint8_t rightDuration, uint8_t rightPower, uint8_t leftDuration, uint8_t leftPower) {
 549         HIDBuffer[3] = rightDuration;
 550         HIDBuffer[4] = rightPower;
 551         HIDBuffer[5] = leftDuration;
 552         HIDBuffer[6] = leftPower;
 553         HID_Command(HIDBuffer, HID_BUFFERSIZE);
 554 }
 555
 556 void PS3BT::setLedRaw(uint8_t value) {
 557         HIDBuffer[11] = value << 1;
 558         HID_Command(HIDBuffer, HID_BUFFERSIZE);
 559 }
 560
 561 void PS3BT::setLedOff(LEDEnum a) {
 562         HIDBuffer[11] &= ~((uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1));
 563         HID_Command(HIDBuffer, HID_BUFFERSIZE);
 564 }
 565
 566 void PS3BT::setLedOn(LEDEnum a) {
 567         if(a == OFF)
 568                 setLedRaw(0);
 569         else {
 570                 HIDBuffer[11] |= (uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1);
 571                 HID_Command(HIDBuffer, HID_BUFFERSIZE);
 572         }
 573 }
 574
 575 void PS3BT::setLedToggle(LEDEnum a) {
 576         HIDBuffer[11] ^= (uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1);
 577         HID_Command(HIDBuffer, HID_BUFFERSIZE);
 578 }
 579
 580 void PS3BT::enable_sixaxis() { // Command used to enable the Dualshock 3 and Navigation controller to send data via Bluetooth
 581         uint8_t cmd_buf[6];
 582         cmd_buf[0] = 0x53; // HID BT Set_report (0x50) | Report Type (Feature 0x03)
 583         cmd_buf[1] = 0xF4; // Report ID
 584         cmd_buf[2] = 0x42; // Special PS3 Controller enable commands
 585         cmd_buf[3] = 0x03;
 586         cmd_buf[4] = 0x00;
 587         cmd_buf[5] = 0x00;
 588
 589         HID_Command(cmd_buf, 6);
 590 }
 591
 592 // Playstation Move Controller commands
 593
 594 void PS3BT::HIDMove_Command(uint8_t* data, uint8_t nbytes) {
 595         if(millis() - timerHID <= 150)// Check if is has been less than 150ms since last command
 596                 delay((uint32_t)(150 - (millis() - timerHID))); // There have to be a delay between commands
 597         pBtd->L2CAP_Command(hci_handle, data, nbytes, interrupt_scid[0], interrupt_scid[1]); // The Move controller sends it's data via the intterrupt channel
 598         timerHID = millis();
 599 }
 600
 601 void PS3BT::moveSetBulb(uint8_t r, uint8_t g, uint8_t b) { // Use this to set the Color using RGB values
 602         // Set the Bulb's values into the write buffer
 603         HIDMoveBuffer[3] = r;
 604         HIDMoveBuffer[4] = g;
 605         HIDMoveBuffer[5] = b;
 606
 607         HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE);
 608 }
 609
 610 void PS3BT::moveSetBulb(ColorsEnum color) { // Use this to set the Color using the predefined colors in enum
 611         moveSetBulb((uint8_t)(color >> 16), (uint8_t)(color >> 8), (uint8_t)(color));
 612 }
 613
 614 void PS3BT::moveSetRumble(uint8_t rumble) {
 615 #ifdef DEBUG_USB_HOST
 616         if(rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100)
 617                 Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"), 0x80);
 618 #endif
 619         // Set the rumble value into the write buffer
 620         HIDMoveBuffer[7] = rumble;
 621
 622         HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE);
 623 }
 624
 625 void PS3BT::onInit() {
 626         if(pFuncOnInit)
 627                 pFuncOnInit(); // Call the user function
 628         else {
 629                 if(PS3MoveConnected)
 630                         moveSetBulb(Red);
 631                 else // Dualshock 3 or Navigation controller
 632                         setLedOn(LED1);
 633         }
 634 }