You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
This repo is archived. You can view files and clone it, but cannot push or open issues/pull-requests.

PS3BT.cpp 31KB

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