PS3BT.cpp

00001 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
00002 
00003  This software may be distributed and modified under the terms of the GNU
00004  General Public License version 2 (GPL2) as published by the Free Software
00005  Foundation and appearing in the file GPL2.TXT included in the packaging of
00006  this file. Please note that GPL2 Section 2[b] requires that all works based
00007  on this software must also be made publicly available under the terms of
00008  the GPL2 ("Copyleft").
00009 
00010  Contact information
00011  -------------------
00012 
00013  Kristian Lauszus, TKJ Electronics
00014  Web      :  http://www.tkjelectronics.com
00015  e-mail   :  kristianl@tkjelectronics.com
00016  */
00017 
00018 #include "PS3BT.h"
00019 // To enable serial debugging see "settings.h"
00020 //#define EXTRADEBUG // Uncomment to get even more debugging data
00021 //#define PRINTREPORT // Uncomment to print the report send by the PS3 Controllers
00022 
00023 PS3BT::PS3BT(BTD *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0) :
00024 BluetoothService(p) // Pointer to USB class instance - mandatory
00025 {
00026         pBtd->my_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead
00027         pBtd->my_bdaddr[4] = btadr4;
00028         pBtd->my_bdaddr[3] = btadr3;
00029         pBtd->my_bdaddr[2] = btadr2;
00030         pBtd->my_bdaddr[1] = btadr1;
00031         pBtd->my_bdaddr[0] = btadr0;
00032 
00033         HIDBuffer[0] = 0x52; // HID BT Set_report (0x50) | Report Type (Output 0x02)
00034         HIDBuffer[1] = 0x01; // Report ID
00035 
00036         // Needed for PS3 Move Controller commands to work via bluetooth
00037         HIDMoveBuffer[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
00038         HIDMoveBuffer[1] = 0x02; // Report ID
00039 
00040         /* Set device cid for the control and intterrupt channelse - LSB */
00041         control_dcid[0] = 0x40; // 0x0040
00042         control_dcid[1] = 0x00;
00043         interrupt_dcid[0] = 0x41; // 0x0041
00044         interrupt_dcid[1] = 0x00;
00045 
00046         Reset();
00047 }
00048 
00049 bool PS3BT::getButtonPress(ButtonEnum b) {
00050         return (ButtonState & pgm_read_dword(&PS3_BUTTONS[(uint8_t)b]));
00051 }
00052 
00053 bool PS3BT::getButtonClick(ButtonEnum b) {
00054         uint32_t button = pgm_read_dword(&PS3_BUTTONS[(uint8_t)b]);
00055         bool click = (ButtonClickState & button);
00056         ButtonClickState &= ~button; // Clear "click" event
00057         return click;
00058 }
00059 
00060 uint8_t PS3BT::getAnalogButton(ButtonEnum a) {
00061         return (uint8_t)(l2capinbuf[pgm_read_byte(&PS3_ANALOG_BUTTONS[(uint8_t)a])]);
00062 }
00063 
00064 uint8_t PS3BT::getAnalogHat(AnalogHatEnum a) {
00065         return (uint8_t)(l2capinbuf[(uint8_t)a + 15]);
00066 }
00067 
00068 int16_t PS3BT::getSensor(SensorEnum a) {
00069         if(PS3Connected) {
00070                 if(a == aX || a == aY || a == aZ || a == gZ)
00071                         return ((l2capinbuf[(uint16_t)a] << 8) | l2capinbuf[(uint16_t)a + 1]);
00072                 else
00073                         return 0;
00074         } else if(PS3MoveConnected) {
00075                 if(a == mXmove || a == mYmove) // These are all 12-bits long
00076                         return (((l2capinbuf[(uint16_t)a] & 0x0F) << 8) | (l2capinbuf[(uint16_t)a + 1]));
00077                 else if(a == mZmove || a == tempMove) // The tempearature is also 12 bits long
00078                         return ((l2capinbuf[(uint16_t)a] << 4) | ((l2capinbuf[(uint16_t)a + 1] & 0xF0) >> 4));
00079                 else // aXmove, aYmove, aZmove, gXmove, gYmove and gZmove
00080                         return (l2capinbuf[(uint16_t)a] | (l2capinbuf[(uint16_t)a + 1] << 8));
00081         } else
00082                 return 0;
00083 }
00084 
00085 float PS3BT::getAngle(AngleEnum a) {
00086         float accXval, accYval, accZval;
00087 
00088         if(PS3Connected) {
00089                 // Data for the Kionix KXPC4 used in the DualShock 3
00090                 const float zeroG = 511.5f; // 1.65/3.3*1023 (1.65V)
00091                 accXval = -((float)getSensor(aX) - zeroG);
00092                 accYval = -((float)getSensor(aY) - zeroG);
00093                 accZval = -((float)getSensor(aZ) - zeroG);
00094         } else if(PS3MoveConnected) {
00095                 // It's a Kionix KXSC4 inside the Motion controller
00096                 const uint16_t zeroG = 0x8000;
00097                 accXval = -(int16_t)(getSensor(aXmove) - zeroG);
00098                 accYval = (int16_t)(getSensor(aYmove) - zeroG);
00099                 accZval = (int16_t)(getSensor(aZmove) - zeroG);
00100         } else
00101                 return 0;
00102 
00103         // Convert to 360 degrees resolution
00104         // atan2 outputs the value of -π to π (radians)
00105         // We are then converting it to 0 to 2π and then to degrees
00106         if(a == Pitch)
00107                 return (atan2f(accYval, accZval) + M_PI) * RAD_TO_DEG;
00108         else
00109                 return (atan2f(accXval, accZval) + M_PI) * RAD_TO_DEG;
00110 }
00111 
00112 float PS3BT::get9DOFValues(SensorEnum a) { // Thanks to Manfred Piendl
00113         if(!PS3MoveConnected)
00114                 return 0;
00115         int16_t value = getSensor(a);
00116         if(a == mXmove || a == mYmove || a == mZmove) {
00117                 if(value > 2047)
00118                         value -= 0x1000;
00119                 return (float)value / 3.2f; // unit: muT = 10^(-6) Tesla
00120         } else if(a == aXmove || a == aYmove || a == aZmove) {
00121                 if(value < 0)
00122                         value += 0x8000;
00123                 else
00124                         value -= 0x8000;
00125                 return (float)value / 442.0f; // unit: m/(s^2)
00126         } else if(a == gXmove || a == gYmove || a == gZmove) {
00127                 if(value < 0)
00128                         value += 0x8000;
00129                 else
00130                         value -= 0x8000;
00131                 if(a == gXmove)
00132                         return (float)value / 11.6f; // unit: deg/s
00133                 else if(a == gYmove)
00134                         return (float)value / 11.2f; // unit: deg/s
00135                 else // gZmove
00136                         return (float)value / 9.6f; // unit: deg/s
00137         } else
00138                 return 0;
00139 }
00140 
00141 char* PS3BT::getTemperature() {
00142         if(PS3MoveConnected) {
00143                 int16_t input = getSensor(tempMove);
00144                 //String output = String(input / 100);
00145                 static char output[16] = {0};
00146                 sprintf(output, "%f", input / 100);
00147 //                output += ".";
00148 //                if(input % 100 < 10)
00149 //                        output += "0";
00150 //                output += String(input % 100);
00151 
00152                 return output;
00153         } else
00154                 return "Error";
00155 }
00156 
00157 bool PS3BT::getStatus(StatusEnum c) {
00158         return (l2capinbuf[(uint16_t)c >> 8] == ((uint8_t)c & 0xff));
00159 }
00160 
00161 void PS3BT::printStatusString() {
00162         char statusOutput[102]; // Max string length plus null character
00163         if(PS3Connected || PS3NavigationConnected) {
00164                 strcpy_P(statusOutput, PSTR("\r\nConnectionStatus: "));
00165 
00166                 if(getStatus(Plugged)) strcat_P(statusOutput, PSTR("Plugged"));
00167                 else if(getStatus(Unplugged)) strcat_P(statusOutput, PSTR("Unplugged"));
00168                 else strcat_P(statusOutput, PSTR("Error"));
00169 
00170                 strcat_P(statusOutput, PSTR(" - PowerRating: "));
00171 
00172                 if(getStatus(Charging)) strcat_P(statusOutput, PSTR("Charging"));
00173                 else if(getStatus(NotCharging)) strcat_P(statusOutput, PSTR("Not Charging"));
00174                 else if(getStatus(Shutdown)) strcat_P(statusOutput, PSTR("Shutdown"));
00175                 else if(getStatus(Dying)) strcat_P(statusOutput, PSTR("Dying"));
00176                 else if(getStatus(Low)) strcat_P(statusOutput, PSTR("Low"));
00177                 else if(getStatus(High)) strcat_P(statusOutput, PSTR("High"));
00178                 else if(getStatus(Full)) strcat_P(statusOutput, PSTR("Full"));
00179                 else strcat_P(statusOutput, PSTR("Error"));
00180 
00181                 strcat_P(statusOutput, PSTR(" - WirelessStatus: "));
00182 
00183                 if(getStatus(CableRumble)) strcat_P(statusOutput, PSTR("Cable - Rumble is on"));
00184                 else if(getStatus(Cable)) strcat_P(statusOutput, PSTR("Cable - Rumble is off"));
00185                 else if(getStatus(BluetoothRumble)) strcat_P(statusOutput, PSTR("Bluetooth - Rumble is on"));
00186                 else if(getStatus(Bluetooth)) strcat_P(statusOutput, PSTR("Bluetooth - Rumble is off"));
00187                 else strcat_P(statusOutput, PSTR("Error"));
00188         } else if(PS3MoveConnected) {
00189                 strcpy_P(statusOutput, PSTR("\r\nPowerRating: "));
00190 
00191                 if(getStatus(MoveCharging)) strcat_P(statusOutput, PSTR("Charging"));
00192                 else if(getStatus(MoveNotCharging)) strcat_P(statusOutput, PSTR("Not Charging"));
00193                 else if(getStatus(MoveShutdown)) strcat_P(statusOutput, PSTR("Shutdown"));
00194                 else if(getStatus(MoveDying)) strcat_P(statusOutput, PSTR("Dying"));
00195                 else if(getStatus(MoveLow)) strcat_P(statusOutput, PSTR("Low"));
00196                 else if(getStatus(MoveHigh)) strcat_P(statusOutput, PSTR("High"));
00197                 else if(getStatus(MoveFull)) strcat_P(statusOutput, PSTR("Full"));
00198                 else strcat_P(statusOutput, PSTR("Error"));
00199         } else
00200                 strcpy_P(statusOutput, PSTR("\r\nError"));
00201 
00202 #ifndef __MBED__
00203         USB_HOST_SERIAL.write(statusOutput);
00204 #endif
00205 }
00206 
00207 void PS3BT::Reset() {
00208         PS3Connected = false;
00209         PS3MoveConnected = false;
00210         PS3NavigationConnected = false;
00211         activeConnection = false;
00212         l2cap_event_flag = 0; // Reset flags
00213         l2cap_state = L2CAP_WAIT;
00214 
00215         // Needed for PS3 Dualshock Controller commands to work via Bluetooth
00216         for(uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++)
00217                 HIDBuffer[i + 2] = pgm_read_byte(&PS3_REPORT_BUFFER[i]); // First two bytes reserved for report type and ID
00218 }
00219 
00220 void PS3BT::disconnect() { // Use this void to disconnect any of the controllers
00221         // First the HID interrupt channel has to be disconnected, then the HID control channel and finally the HCI connection
00222         pBtd->l2cap_disconnection_request(hci_handle, ++identifier, interrupt_scid, interrupt_dcid);
00223         Reset();
00224         l2cap_state = L2CAP_INTERRUPT_DISCONNECT;
00225 }
00226 
00227 void PS3BT::ACLData(uint8_t* ACLData) {
00228         if(!pBtd->l2capConnectionClaimed && !PS3Connected && !PS3MoveConnected && !PS3NavigationConnected && !activeConnection && !pBtd->connectToWii && !pBtd->incomingWii && !pBtd->pairWithWii) {
00229                 if(ACLData[8] == L2CAP_CMD_CONNECTION_REQUEST) {
00230                         if((ACLData[12] | (ACLData[13] << 8)) == HID_CTRL_PSM) {
00231                                 pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service
00232                                 activeConnection = true;
00233                                 hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
00234                                 l2cap_state = L2CAP_WAIT;
00235                                 remote_name_first = pBtd->remote_name[0]; // Store the first letter in remote name for the connection
00236 #ifdef DEBUG_USB_HOST
00237                                 if(pBtd->hci_version < 3) { // Check the HCI Version of the Bluetooth dongle
00238                                         Notify(PSTR("\r\nYour dongle may not support reading the analog buttons, sensors and status\r\nYour HCI Version is: "), 0x80);
00239                                         Notify(pBtd->hci_version, 0x80);
00240                                         Notify(PSTR("\r\nBut should be at least 3\r\nThis means that it doesn't support Bluetooth Version 2.0+EDR"), 0x80);
00241                                 }
00242 #endif
00243                         }
00244                 }
00245         }
00246 
00247         if(checkHciHandle(ACLData, hci_handle)) { // acl_handle_ok
00248                 memcpy(l2capinbuf, ACLData, BULK_MAXPKTSIZE);
00249                 if((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001U) { // l2cap_control - Channel ID for ACL-U
00250                         if(l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
00251 #ifdef DEBUG_USB_HOST
00252                                 Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80);
00253                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
00254                                 Notify(PSTR(" "), 0x80);
00255                                 D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
00256                                 Notify(PSTR(" Data: "), 0x80);
00257                                 D_PrintHex<uint8_t > (l2capinbuf[17], 0x80);
00258                                 Notify(PSTR(" "), 0x80);
00259                                 D_PrintHex<uint8_t > (l2capinbuf[16], 0x80);
00260                                 Notify(PSTR(" "), 0x80);
00261                                 D_PrintHex<uint8_t > (l2capinbuf[15], 0x80);
00262                                 Notify(PSTR(" "), 0x80);
00263                                 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
00264 #endif
00265                         } else if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
00266 #ifdef EXTRADEBUG
00267                                 Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80);
00268                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
00269                                 Notify(PSTR(" "), 0x80);
00270                                 D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
00271                                 Notify(PSTR(" SCID: "), 0x80);
00272                                 D_PrintHex<uint8_t > (l2capinbuf[15], 0x80);
00273                                 Notify(PSTR(" "), 0x80);
00274                                 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
00275                                 Notify(PSTR(" Identifier: "), 0x80);
00276                                 D_PrintHex<uint8_t > (l2capinbuf[9], 0x80);
00277 #endif
00278                                 if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
00279                                         identifier = l2capinbuf[9];
00280                                         control_scid[0] = l2capinbuf[14];
00281                                         control_scid[1] = l2capinbuf[15];
00282                                         l2cap_set_flag(L2CAP_FLAG_CONNECTION_CONTROL_REQUEST);
00283                                 } else if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) {
00284                                         identifier = l2capinbuf[9];
00285                                         interrupt_scid[0] = l2capinbuf[14];
00286                                         interrupt_scid[1] = l2capinbuf[15];
00287                                         l2cap_set_flag(L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST);
00288                                 }
00289                         } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
00290                                 if((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
00291                                         if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
00292                                                 //Notify(PSTR("\r\nHID Control Configuration Complete"), 0x80);
00293                                                 l2cap_set_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS);
00294                                         } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
00295                                                 //Notify(PSTR("\r\nHID Interrupt Configuration Complete"), 0x80);
00296                                                 l2cap_set_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS);
00297                                         }
00298                                 }
00299                         } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
00300                                 if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
00301                                         //Notify(PSTR("\r\nHID Control Configuration Request"), 0x80);
00302                                         pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], control_scid);
00303                                 } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
00304                                         //Notify(PSTR("\r\nHID Interrupt Configuration Request"), 0x80);
00305                                         pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], interrupt_scid);
00306                                 }
00307                         } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
00308                                 if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
00309 #ifdef DEBUG_USB_HOST
00310                                         Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80);
00311 #endif
00312                                         identifier = l2capinbuf[9];
00313                                         pBtd->l2cap_disconnection_response(hci_handle, identifier, control_dcid, control_scid);
00314                                         Reset();
00315                                 } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
00316 #ifdef DEBUG_USB_HOST
00317                                         Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80);
00318 #endif
00319                                         identifier = l2capinbuf[9];
00320                                         pBtd->l2cap_disconnection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid);
00321                                         Reset();
00322                                 }
00323                         } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
00324                                 if(l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
00325                                         //Notify(PSTR("\r\nDisconnect Response: Control Channel"), 0x80);
00326                                         identifier = l2capinbuf[9];
00327                                         l2cap_set_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE);
00328                                 } else if(l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) {
00329                                         //Notify(PSTR("\r\nDisconnect Response: Interrupt Channel"), 0x80);
00330                                         identifier = l2capinbuf[9];
00331                                         l2cap_set_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE);
00332                                 }
00333                         }
00334 #ifdef EXTRADEBUG
00335                         else {
00336                                 Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80);
00337                                 D_PrintHex<uint8_t > (l2capinbuf[8], 0x80);
00338                         }
00339 #endif
00340                 } else if(l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
00341                         //Notify(PSTR("\r\nL2CAP Interrupt"), 0x80);
00342                         if(PS3Connected || PS3MoveConnected || PS3NavigationConnected) {
00343                                 /* Read Report */
00344                                 if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
00345                                         lastMessageTime = (uint32_t)millis(); // Store the last message time
00346 
00347                                         if(PS3Connected || PS3NavigationConnected)
00348                                                 ButtonState = (uint32_t)(l2capinbuf[11] | ((uint16_t)l2capinbuf[12] << 8) | ((uint32_t)l2capinbuf[13] << 16));
00349                                         else if(PS3MoveConnected)
00350                                                 ButtonState = (uint32_t)(l2capinbuf[10] | ((uint16_t)l2capinbuf[11] << 8) | ((uint32_t)l2capinbuf[12] << 16));
00351 
00352                                         //Notify(PSTR("\r\nButtonState", 0x80);
00353                                         //PrintHex<uint32_t>(ButtonState, 0x80);
00354 
00355                                         if(ButtonState != OldButtonState) {
00356                                                 ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable
00357                                                 OldButtonState = ButtonState;
00358                                         }
00359 
00360 #ifdef PRINTREPORT // Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers
00361                                         for(uint8_t i = 10; i < 58; i++) {
00362                                                 D_PrintHex<uint8_t > (l2capinbuf[i], 0x80);
00363                                                 Notify(PSTR(" "), 0x80);
00364                                         }
00365                                         Notify(PSTR("\r\n"), 0x80);
00366 #endif
00367                                 }
00368                         }
00369                 }
00370                 L2CAP_task();
00371         }
00372 }
00373 
00374 void PS3BT::L2CAP_task() {
00375         switch(l2cap_state) {
00376                 case L2CAP_WAIT:
00377                         if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_CONTROL_REQUEST)) {
00378 #ifdef DEBUG_USB_HOST
00379                                 Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80);
00380 #endif
00381                                 pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, PENDING);
00382                                 wait_ms(1);
00383                                 pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, SUCCESSFUL);
00384                                 identifier++;
00385                                 wait_ms(1);
00386                                 pBtd->l2cap_config_request(hci_handle, identifier, control_scid);
00387                                 l2cap_state = L2CAP_CONTROL_SUCCESS;
00388                         }
00389                         break;
00390 
00391                 case L2CAP_CONTROL_SUCCESS:
00392                         if(l2cap_check_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)) {
00393 #ifdef DEBUG_USB_HOST
00394                                 Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80);
00395 #endif
00396                                 l2cap_state = L2CAP_INTERRUPT_SETUP;
00397                         }
00398                         break;
00399 
00400                 case L2CAP_INTERRUPT_SETUP:
00401                         if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST)) {
00402 #ifdef DEBUG_USB_HOST
00403                                 Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80);
00404 #endif
00405                                 pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, PENDING);
00406                                 wait_ms(1);
00407                                 pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, SUCCESSFUL);
00408                                 identifier++;
00409                                 wait_ms(1);
00410                                 pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid);
00411 
00412                                 l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST;
00413                         }
00414                         break;
00415 
00416                 case L2CAP_INTERRUPT_CONFIG_REQUEST:
00417                         if(l2cap_check_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS)) { // Now the HID channels is established
00418 #ifdef DEBUG_USB_HOST
00419                                 Notify(PSTR("\r\nHID Interrupt Successfully Configured"), 0x80);
00420 #endif
00421                                 if(remote_name_first == 'M') { // First letter in Motion Controller ('M')
00422                                         memset(l2capinbuf, 0, BULK_MAXPKTSIZE); // Reset l2cap in buffer as it sometimes read it as a button has been pressed
00423                                         l2cap_state = TURN_ON_LED;
00424                                 } else
00425                                         l2cap_state = PS3_ENABLE_SIXAXIS;
00426                                 timer = (uint32_t)millis();
00427                         }
00428                         break;
00429 
00430                         /* These states are handled in Run() */
00431 
00432                 case L2CAP_INTERRUPT_DISCONNECT:
00433                         if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE)) {
00434 #ifdef DEBUG_USB_HOST
00435                                 Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80);
00436 #endif
00437                                 identifier++;
00438                                 pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid);
00439                                 l2cap_state = L2CAP_CONTROL_DISCONNECT;
00440                         }
00441                         break;
00442 
00443                 case L2CAP_CONTROL_DISCONNECT:
00444                         if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE)) {
00445 #ifdef DEBUG_USB_HOST
00446                                 Notify(PSTR("\r\nDisconnected Control Channel"), 0x80);
00447 #endif
00448                                 pBtd->hci_disconnect(hci_handle);
00449                                 hci_handle = -1; // Reset handle
00450                                 l2cap_event_flag = 0; // Reset flags
00451                                 l2cap_state = L2CAP_WAIT;
00452                         }
00453                         break;
00454         }
00455 }
00456 
00457 void PS3BT::Run() {
00458         switch(l2cap_state) {
00459                 case PS3_ENABLE_SIXAXIS:
00460                         if((int32_t)((uint32_t)millis() - timer) > 1000) { // loop 1 second before sending the command
00461                                 memset(l2capinbuf, 0, BULK_MAXPKTSIZE); // Reset l2cap in buffer as it sometimes read it as a button has been pressed
00462                                 for(uint8_t i = 15; i < 19; i++)
00463                                         l2capinbuf[i] = 0x7F; // Set the analog joystick values to center position
00464                                 enable_sixaxis();
00465                                 l2cap_state = TURN_ON_LED;
00466                                 timer = (uint32_t)millis();
00467                         }
00468                         break;
00469 
00470                 case TURN_ON_LED:
00471                         if((int32_t)((uint32_t)millis() - timer) > 1000) { // loop 1 second before sending the command
00472                                 if(remote_name_first == 'P') { // First letter in PLAYSTATION(R)3 Controller ('P')
00473 #ifdef DEBUG_USB_HOST
00474                                         Notify(PSTR("\r\nDualshock 3 Controller Enabled\r\n"), 0x80);
00475 #endif
00476                                         PS3Connected = true;
00477                                 } else if(remote_name_first == 'N') { // First letter in Navigation Controller ('N')
00478 #ifdef DEBUG_USB_HOST
00479                                         Notify(PSTR("\r\nNavigation Controller Enabled\r\n"), 0x80);
00480 #endif
00481                                         PS3NavigationConnected = true;
00482                                 } else if(remote_name_first == 'M') { // First letter in Motion Controller ('M')
00483                                         timer = (uint32_t)millis();
00484 #ifdef DEBUG_USB_HOST
00485                                         Notify(PSTR("\r\nMotion Controller Enabled\r\n"), 0x80);
00486 #endif
00487                                         PS3MoveConnected = true;
00488                                 }
00489                                 ButtonState = 0; // Clear all values
00490                                 OldButtonState = 0;
00491                                 ButtonClickState = 0;
00492 
00493                                 onInit(); // Turn on the LED on the controller
00494                                 l2cap_state = L2CAP_DONE;
00495                         }
00496                         break;
00497 
00498                 case L2CAP_DONE:
00499                         if(PS3MoveConnected) { // The Bulb and rumble values, has to be send at approximately every 5th second for it to stay on
00500                                 if((int32_t)((uint32_t)millis() - timer) > 4000) { // Send at least every 4th second
00501                                         HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE); // The Bulb and rumble values, has to be written again and again, for it to stay turned on
00502                                         timer = (uint32_t)millis();
00503                                 }
00504                         }
00505                         break;
00506         }
00507 }
00508 
00509 /************************************************************/
00510 /*                    HID Commands                          */
00511 /************************************************************/
00512 
00513 // Playstation Sixaxis Dualshock and Navigation Controller commands
00514 
00515 void PS3BT::HID_Command(uint8_t* data, uint8_t nbytes) {
00516         if((int32_t)((uint32_t)millis() - timerHID) <= 150) // Check if is has been more than 150ms since last command
00517                 wait_ms((uint32_t)(150 - ((uint32_t)millis() - timerHID))); // There have to be a delay between commands
00518         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
00519         timerHID = (uint32_t)millis();
00520 }
00521 
00522 void PS3BT::setAllOff() {
00523         HIDBuffer[3] = 0x00; // Rumble bytes
00524         HIDBuffer[4] = 0x00;
00525         HIDBuffer[5] = 0x00;
00526         HIDBuffer[6] = 0x00;
00527 
00528         HIDBuffer[11] = 0x00; // LED byte
00529 
00530         HID_Command(HIDBuffer, HID_BUFFERSIZE);
00531 }
00532 
00533 void PS3BT::setRumbleOff() {
00534         uint8_t rumbleBuf[HID_BUFFERSIZE];
00535         memcpy(rumbleBuf, HIDBuffer, HID_BUFFERSIZE);
00536         rumbleBuf[3] = 0x00;
00537         rumbleBuf[4] = 0x00;
00538         rumbleBuf[5] = 0x00;
00539         rumbleBuf[6] = 0x00;
00540         HID_Command(rumbleBuf, HID_BUFFERSIZE);
00541 }
00542 
00543 void PS3BT::setRumbleOn(RumbleEnum mode) {
00544         uint8_t power[2] = {0xff, 0x00}; // Defaults to RumbleLow
00545         if(mode == RumbleHigh) {
00546                 power[0] = 0x00;
00547                 power[1] = 0xff;
00548         }
00549         setRumbleOn(0xfe, power[0], 0xfe, power[1]);
00550 }
00551 
00552 void PS3BT::setRumbleOn(uint8_t rightDuration, uint8_t rightPower, uint8_t leftDuration, uint8_t leftPower) {
00553         uint8_t rumbleBuf[HID_BUFFERSIZE];
00554         memcpy(rumbleBuf, HIDBuffer, HID_BUFFERSIZE);
00555         rumbleBuf[3] = rightDuration;
00556         rumbleBuf[4] = rightPower;
00557         rumbleBuf[5] = leftDuration;
00558         rumbleBuf[6] = leftPower;
00559         HID_Command(rumbleBuf, HID_BUFFERSIZE);
00560 }
00561 
00562 void PS3BT::setLedRaw(uint8_t value) {
00563         HIDBuffer[11] = value << 1;
00564         HID_Command(HIDBuffer, HID_BUFFERSIZE);
00565 }
00566 
00567 void PS3BT::setLedOff(LEDEnum a) {
00568         HIDBuffer[11] &= ~((uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1));
00569         HID_Command(HIDBuffer, HID_BUFFERSIZE);
00570 }
00571 
00572 void PS3BT::setLedOn(LEDEnum a) {
00573         if(a == OFF)
00574                 setLedRaw(0);
00575         else {
00576                 HIDBuffer[11] |= (uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1);
00577                 HID_Command(HIDBuffer, HID_BUFFERSIZE);
00578         }
00579 }
00580 
00581 void PS3BT::setLedToggle(LEDEnum a) {
00582         HIDBuffer[11] ^= (uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1);
00583         HID_Command(HIDBuffer, HID_BUFFERSIZE);
00584 }
00585 
00586 void PS3BT::enable_sixaxis() { // Command used to enable the Dualshock 3 and Navigation controller to send data via Bluetooth
00587         uint8_t cmd_buf[6];
00588         cmd_buf[0] = 0x53; // HID BT Set_report (0x50) | Report Type (Feature 0x03)
00589         cmd_buf[1] = 0xF4; // Report ID
00590         cmd_buf[2] = 0x42; // Special PS3 Controller enable commands
00591         cmd_buf[3] = 0x03;
00592         cmd_buf[4] = 0x00;
00593         cmd_buf[5] = 0x00;
00594 
00595         HID_Command(cmd_buf, 6);
00596 }
00597 
00598 // Playstation Move Controller commands
00599 
00600 void PS3BT::HIDMove_Command(uint8_t* data, uint8_t nbytes) {
00601         if((int32_t)((uint32_t)millis() - timerHID) <= 150)// Check if is has been less than 150ms since last command
00602                 wait_ms((uint32_t)(150 - ((uint32_t)millis() - timerHID))); // There have to be a delay between commands
00603         pBtd->L2CAP_Command(hci_handle, data, nbytes, interrupt_scid[0], interrupt_scid[1]); // The Move controller sends it's data via the intterrupt channel
00604         timerHID = (uint32_t)millis();
00605 }
00606 
00607 void PS3BT::moveSetBulb(uint8_t r, uint8_t g, uint8_t b) { // Use this to set the Color using RGB values
00608         // Set the Bulb's values into the write buffer
00609         HIDMoveBuffer[3] = r;
00610         HIDMoveBuffer[4] = g;
00611         HIDMoveBuffer[5] = b;
00612 
00613         HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE);
00614 }
00615 
00616 void PS3BT::moveSetBulb(ColorsEnum color) { // Use this to set the Color using the predefined colors in enum
00617         moveSetBulb((uint8_t)(color >> 16), (uint8_t)(color >> 8), (uint8_t)(color));
00618 }
00619 
00620 void PS3BT::moveSetRumble(uint8_t rumble) {
00621 #ifdef DEBUG_USB_HOST
00622         if(rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100)
00623                 Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"), 0x80);
00624 #endif
00625         // Set the rumble value into the write buffer
00626         HIDMoveBuffer[7] = rumble;
00627 
00628         HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE);
00629 }
00630 
00631 void PS3BT::onInit() {
00632         if(pFuncOnInit)
00633                 pFuncOnInit(); // Call the user function
00634         else {
00635                 if(PS3MoveConnected)
00636                         moveSetBulb(Red);
00637                 else // Dualshock 3 or Navigation controller
00638                         setLedOn(static_cast<LEDEnum>(LED1));
00639         }
00640 }