SHARE
TWEET

Untitled

a guest Aug 29th, 2012 78 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  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 "Wii.h"
  19. #define DEBUG // Uncomment to print data for debugging
  20. #define EXTRADEBUG // Uncomment to get even more debugging data
  21. #define PRINTREPORT // Uncomment to print the report send by the Wiimote
  22.  
  23. WII::WII(BTD *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0):
  24. pBtd(p) // pointer to USB class instance - mandatory
  25. {
  26.     if (pBtd)
  27.         pBtd->wiiServiceID = pBtd->registerServiceClass(this); // Register it as a Bluetooth service
  28.    
  29.     pBtd->disc_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead
  30.     pBtd->disc_bdaddr[4] = btadr4;
  31.     pBtd->disc_bdaddr[3] = btadr3;
  32.     pBtd->disc_bdaddr[2] = btadr2;
  33.     pBtd->disc_bdaddr[1] = btadr1;
  34.     pBtd->disc_bdaddr[0] = btadr0;
  35.            
  36.     HIDBuffer[0] = 0xA2;// HID BT DATA_request (0x50) | Report Type (Output 0x02)
  37.    
  38.     /* Set device cid for the control and intterrupt channelse - LSB */
  39.     control_dcid[0] = 0x60;//0x0060
  40.     control_dcid[1] = 0x00;
  41.     interrupt_dcid[0] = 0x61;//0x0061
  42.     interrupt_dcid[1] = 0x00;
  43.    
  44.     Reset();
  45. }
  46. void WII::Reset() {
  47.     connected = false;
  48.     l2cap_event_flag = 0; // Reset flags
  49.     l2cap_state = L2CAP_WAIT;
  50. }
  51.  
  52. void WII::disconnect() { // Use this void to disconnect any of the controllers
  53.     //First the HID interrupt channel has to be disconencted, then the HID control channel and finally the HCI connection
  54.     pBtd->l2cap_disconnection_request(hci_handle,0x0A, interrupt_scid, interrupt_dcid);
  55.     Reset();
  56.     l2cap_state = L2CAP_INTERRUPT_DISCONNECT;
  57. }
  58.  
  59. void WII::ACLData(uint8_t* l2capinbuf) {
  60.     if (((l2capinbuf[0] | (l2capinbuf[1] << 8)) == (hci_handle | 0x2000))) { //acl_handle_ok
  61.         if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U
  62.             if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
  63. #ifdef DEBUG
  64.                 Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "));
  65.                 PrintHex<uint8_t>(l2capinbuf[13]);
  66.                 Notify(PSTR(" "));
  67.                 PrintHex<uint8_t>(l2capinbuf[12]);
  68.                 Notify(PSTR(" "));
  69.                 PrintHex<uint8_t>(l2capinbuf[17]);
  70.                 Notify(PSTR(" "));
  71.                 PrintHex<uint8_t>(l2capinbuf[16]);
  72.                 Notify(PSTR(" "));
  73.                 PrintHex<uint8_t>(l2capinbuf[15]);
  74.                 Notify(PSTR(" "));
  75.                 PrintHex<uint8_t>(l2capinbuf[14]);
  76. #endif
  77.             }
  78.             else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_RESPONSE) {
  79.                         Serial.print("\r\nL2CAP_CMD_CONNECTION_RESPONSE");
  80.                 if (((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) && ((l2capinbuf[18] | (l2capinbuf[19] << 8)) == SUCCESSFUL)) { // Success
  81.                     if (l2capinbuf[14] == control_dcid[0] && l2capinbuf[15] == control_dcid[1]) { // Success
  82.                         //Serial.print("\r\nHID Control Connection Complete");
  83.                         identifier = l2capinbuf[9];
  84.                         control_scid[0] = l2capinbuf[12];
  85.                         control_scid[1] = l2capinbuf[13];
  86.                         l2cap_event_flag |= L2CAP_FLAG_CONTROL_CONNECTED;
  87.                     }
  88.                     else if (l2capinbuf[14] == interrupt_dcid[0] && l2capinbuf[15] == interrupt_dcid[1]) {
  89.                         //Serial.print("\r\nHID Interrupt Connection Complete");
  90.                         identifier = l2capinbuf[9];
  91.                         interrupt_scid[0] = l2capinbuf[12];
  92.                         interrupt_scid[1] = l2capinbuf[13];
  93.                         l2cap_event_flag |= L2CAP_FLAG_INTERRUPT_CONNECTED;
  94.                     }
  95.                 }
  96.             }
  97.             else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
  98.                 if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
  99.                     if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
  100.                         //Serial.print("\r\nHID Control Configuration Complete");
  101.                         identifier = l2capinbuf[9];
  102.                         l2cap_event_flag |= L2CAP_FLAG_CONFIG_CONTROL_SUCCESS;
  103.                     }
  104.                     else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
  105.                         //Serial.print("\r\nHID Interrupt Configuration Complete");
  106.                         identifier = l2capinbuf[9];                        
  107.                         l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS;
  108.                     }
  109.                 }
  110.             }
  111.             else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
  112.                 if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
  113.                     //Serial.print("\r\nHID Control Configuration Request");
  114.                     pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], control_scid);
  115.                 }
  116.                 else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
  117.                     //Serial.print("\r\nHID Interrupt Configuration Request");
  118.                     pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], interrupt_scid);
  119.                 }
  120.             }
  121.             else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
  122.                 if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
  123. #ifdef DEBUG
  124.                     Notify(PSTR("\r\nDisconnect Request: Control Channel"));
  125. #endif
  126.                     connected = false;
  127.                     identifier = l2capinbuf[9];
  128.                     pBtd->l2cap_disconnection_response(hci_handle,identifier,control_dcid,control_scid);                    
  129.                     Reset();
  130.                 }
  131.                 else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
  132. #ifdef DEBUG
  133.                     Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"));
  134. #endif
  135.                     connected = false;
  136.                     identifier = l2capinbuf[9];
  137.                     pBtd->l2cap_disconnection_response(hci_handle,identifier,interrupt_dcid,interrupt_scid);                    
  138.                     Reset();
  139.                 }
  140.             }
  141.             else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
  142.                 if (l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
  143.                     //Serial.print("\r\nDisconnect Response: Control Channel");
  144.                     identifier = l2capinbuf[9];
  145.                     l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE;
  146.                 }
  147.                 else if (l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) {
  148.                     //Serial.print("\r\nDisconnect Response: Interrupt Channel");
  149.                     identifier = l2capinbuf[9];
  150.                     l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE;
  151.                 }
  152.             }
  153. #ifdef EXTRADEBUG
  154.             else {
  155.                 identifier = l2capinbuf[9];
  156.                 Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "));
  157.                 PrintHex<uint8_t>(l2capinbuf[8]);
  158.             }
  159. #endif
  160.         } else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
  161.             //Serial.print("\r\nL2CAP Interrupt");
  162.             if(connected) {
  163.                 /* Read Report */
  164.                 if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
  165.                     if(l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) { // These reports include the buttons
  166.                         ButtonState = (uint16_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
  167.                         ButtonClickState = ButtonState; // Update click state variable                        
  168. #ifdef PRINTREPORT
  169.                         Notify(PSTR("ButtonState: "));
  170.                         PrintHex<uint16_t>(ButtonState);
  171.                         Notify(PSTR("\r\n"));
  172. #endif
  173.                         if(ButtonState != OldButtonState) {
  174.                             buttonChanged = true;
  175.                             if(ButtonState != 0x0000) {
  176.                                 buttonPressed = true;
  177.                                 buttonReleased = false;
  178.                             } else {
  179.                                 buttonPressed = false;
  180.                                 buttonReleased = true;
  181.                             }
  182.                         }
  183.                         else {
  184.                             buttonChanged = false;
  185.                             buttonPressed = false;
  186.                             buttonReleased = false;
  187.                         }
  188.                         OldButtonState = ButtonState;
  189.                     }
  190.                     if(l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x35) { // Read the accelerometer
  191.                         int16_t accX = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5))-500;
  192.                         int16_t accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4))-500;
  193.                         int16_t accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5))-500;
  194.                         /*
  195.                         Notify(PSTR("\r\naccX: "));
  196.                         Serial.print(accX);
  197.                         Notify(PSTR("\taccY: "));
  198.                         Serial.print(accY);
  199.                         Notify(PSTR("\taccZ: "));
  200.                         Serial.print(accZ);
  201.                         */                        
  202.                         pitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  203.                         roll = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  204.                         /*
  205.                         Notify(PSTR("\r\nPitch: "));
  206.                         Serial.print(pitch);
  207.                         Notify(PSTR("\tRoll: "));
  208.                         Serial.print(roll);
  209.                         */
  210.                     }
  211.                     switch (l2capinbuf[9]) {
  212.                         case 0x20: // Status Information
  213.                             // (a1) 20 BB BB LF 00 00 VV
  214.                             if(l2capinbuf[12] & 0x02) // Check if a extension is connected
  215.                                 setReportMode(false,0x35); // Also read the extension
  216.                             else
  217.                                 setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer
  218.                             break;
  219.                         case 0x30: // Core buttons
  220.                             // (a1) 30 BB BB
  221.                             break;
  222.                         case 0x31: // Core Buttons and Accelerometer
  223.                             // (a1) 31 BB BB AA AA AA
  224.                             break;
  225.                         case 0x32: // Core Buttons with 8 Extension bytes
  226.                             // (a1) 32 BB BB EE EE EE EE EE EE EE EE
  227.                             /*
  228.                             Notify(PSTR("\r\n"));
  229.                             for (uint8_t i = 0; i < 8; i++) {
  230.                                 Serial.print(l2capinbuf[12+i]);
  231.                                 Notify(PSTR(" "));
  232.                             }                    
  233.                             */                            
  234.                             break;
  235.                         case 0x34: // Core Buttons with 19 Extension bytes
  236.                             // (a1) 34 BB BB EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
  237.                             /*
  238.                             Notify(PSTR("\r\n"));
  239.                             for (uint8_t i = 0; i < 19; i++) {
  240.                                 Serial.print(l2capinbuf[12+i]);
  241.                                 Notify(PSTR(" "));
  242.                             }
  243.                             */
  244.                             break;
  245.                         case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes
  246.                             // (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
  247.                             /*
  248.                             Notify(PSTR("\r\n"));
  249.                             for (uint8_t i = 0; i < 16; i++) {
  250.                                 Serial.print(l2capinbuf[15+i]);
  251.                                 Notify(PSTR(" "));
  252.                             }
  253.                             */
  254.                             break;
  255. #ifdef DEBUG
  256.                         default:
  257.                             Notify(PSTR("\r\nUnknown Report type: "));
  258.                             Serial.print(l2capinbuf[9],HEX);
  259.                             break;
  260. #endif
  261.                     }                    
  262.                 }
  263.             }
  264.         }
  265.         L2CAP_task();
  266.     }
  267. }
  268. void WII::L2CAP_task() {
  269.     switch (l2cap_state) {
  270.         case L2CAP_CONTROL_CONNECT_REQUEST:
  271.             if (l2cap_connected_control_flag) {
  272. #ifdef DEBUG
  273.                 Notify(PSTR("\r\nSend HID Control Config Request"));
  274. #endif
  275.                 identifier++;
  276.                 pBtd->l2cap_config_request(hci_handle, identifier, control_scid);
  277.                 l2cap_state = L2CAP_CONTROL_CONFIG_REQUEST;
  278.             }
  279.             break;
  280.            
  281.         case L2CAP_CONTROL_CONFIG_REQUEST:
  282.             if(l2cap_config_success_control_flag) {
  283. #ifdef DEBUG
  284.                 Notify(PSTR("\r\nSend HID Interrupt Connection Request"));
  285. #endif
  286.                 identifier++;
  287.                 pBtd->l2cap_connection_request(hci_handle,identifier,interrupt_dcid,HID_INTR_PSM);
  288.                 l2cap_state = L2CAP_INTERRUPT_CONNECT_REQUEST;
  289.             }
  290.             break;
  291.            
  292.         case L2CAP_INTERRUPT_CONNECT_REQUEST:
  293.             if(l2cap_connected_interrupt_flag) {
  294. #ifdef DEBUG
  295.                 Notify(PSTR("\r\nSend HID Interrupt Config Request"));
  296. #endif
  297.                 identifier++;
  298.                 pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid);
  299.                 l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST;
  300.             }
  301.             break;
  302.            
  303.            
  304.         case L2CAP_INTERRUPT_CONFIG_REQUEST:
  305.             if(l2cap_config_success_interrupt_flag) {
  306. #ifdef DEBUG
  307.                 Notify(PSTR("\r\nHID Channels Established"));
  308. #endif
  309.                 statusRequest();
  310.                 l2cap_state = L2CAP_WII_STATUS_STATE;
  311.             }
  312.             break;
  313.            
  314.         case L2CAP_WII_STATUS_STATE:                        
  315.             connected = true;
  316.             pBtd->connectToWii = false;
  317.             ButtonState = 0;
  318.             OldButtonState = 0;
  319.             ButtonClickState = 0;
  320.             setLedOn(LED1);
  321.             l2cap_state = L2CAP_DONE;
  322.             break;
  323. /*
  324.         case L2CAP_WIIREMOTE_CAL_STATE:
  325.             //Todo enable support for Motion Plus
  326.             break;
  327. */
  328.         case L2CAP_DONE:
  329.             break;
  330.            
  331.         case L2CAP_INTERRUPT_DISCONNECT:
  332.             if (l2cap_disconnect_response_interrupt_flag) {
  333. #ifdef DEBUG
  334.                 Notify(PSTR("\r\nDisconnected Interrupt Channel"));
  335. #endif
  336.                 identifier++;
  337.                 pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid);                
  338.                 l2cap_state = L2CAP_CONTROL_DISCONNECT;
  339.             }
  340.             break;
  341.            
  342.         case L2CAP_CONTROL_DISCONNECT:
  343.             if (l2cap_disconnect_response_control_flag) {
  344. #ifdef DEBUG
  345.                 Notify(PSTR("\r\nDisconnected Control Channel"));
  346. #endif
  347.                 pBtd->hci_disconnect(hci_handle);
  348.                 l2cap_event_flag = 0; // Reset flags
  349.                 l2cap_state = L2CAP_WAIT;
  350.             }
  351.             break;
  352.     }    
  353. }
  354. void WII::Run() {
  355.     switch (l2cap_state) {
  356.         case L2CAP_WAIT:
  357.             if(pBtd->connectToWii) {
  358. #ifdef DEBUG
  359.                 Notify(PSTR("\r\nSend HID Control Connection Request"));
  360. #endif
  361.                 hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection                
  362.                 l2cap_event_flag = 0; // Reset flags
  363.                 identifier = 0;
  364.                 pBtd->l2cap_connection_request(hci_handle,identifier,control_dcid,HID_CTRL_PSM);
  365.                 l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST;                
  366.             }
  367.             break;
  368.     }
  369. }
  370.  
  371. /************************************************************/
  372. /*                    HID Commands                          */
  373. /************************************************************/
  374. void WII::HID_Command(uint8_t* data, uint8_t nbytes) {
  375.     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
  376. }
  377. void WII::setAllOff() {
  378.     HIDBuffer[1] = 0x11;
  379.     HIDBuffer[2] = 0x00;
  380.     HID_Command(HIDBuffer, 3);
  381. }
  382. void WII::setRumbleOff() {
  383.     HIDBuffer[1] = 0x11;
  384.     HIDBuffer[2] &= ~0x01; // Bit 0 control the rumble
  385.     HID_Command(HIDBuffer, 3);    
  386. }
  387. void WII::setRumbleOn() {
  388.     HIDBuffer[1] = 0x11;
  389.     HIDBuffer[2] |= 0x01; // Bit 0 control the rumble
  390.     HID_Command(HIDBuffer, 3);
  391. }
  392. void WII::setRumbleToggle() {
  393.     HIDBuffer[1] = 0x11;
  394.     HIDBuffer[2] ^= 0x01; // Bit 0 control the rumble
  395.     HID_Command(HIDBuffer, 3);
  396. }
  397. void WII::setLedOff(LED a) {
  398.     HIDBuffer[1] = 0x11;
  399.     HIDBuffer[2] &= ~((uint8_t)a);
  400.     HID_Command(HIDBuffer, 3);    
  401. }
  402. void WII::setLedOn(LED a) {
  403.     HIDBuffer[1] = 0x11;
  404.     HIDBuffer[2] |= (uint8_t)a;
  405.     HID_Command(HIDBuffer, 3);
  406. }
  407. void WII::setLedToggle(LED a) {
  408.     HIDBuffer[1] = 0x11;
  409.     HIDBuffer[2] ^= (uint8_t)a;
  410.     HID_Command(HIDBuffer, 3);
  411. }
  412. void WII::setReportMode(bool continuous, uint8_t mode) {
  413.     uint8_t cmd_buf[4];
  414.     cmd_buf[0] = 0xA2; // HID BT DATA_request (0x50) | Report Type (Output 0x02)
  415.     cmd_buf[1] = 0x12;
  416.     if(continuous)
  417.         cmd_buf[2] = 0x04;
  418.     else
  419.         cmd_buf[2] = 0x00;
  420.     cmd_buf[3] = mode;
  421.     HID_Command(cmd_buf, 4);
  422. }
  423. void WII::statusRequest() {
  424.     uint8_t cmd_buf[3];
  425.     cmd_buf[0] = 0xA2; // HID BT DATA_request (0x50) | Report Type (Output 0x02)
  426.     cmd_buf[1] = 0x15;
  427.     cmd_buf[2] = 0x00;
  428.     HID_Command(cmd_buf, 3);
  429. }
  430.  
  431. /************************************************************/
  432. /*                    WII Commands                          */
  433. /************************************************************/
  434.  
  435. bool WII::getButtonPress(Button b) {    
  436.     if(ButtonState & (uint16_t)b)
  437.         return true;
  438.     else
  439.         return false;
  440. }
  441. bool WII::getButtonClick(Button b) {
  442.     bool click = ((ButtonClickState & (uint16_t)b) != 0);
  443.     ButtonClickState &= ~((uint16_t)b);  // clear "click" event
  444.     return click;
  445. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top