API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Aug 29th, 2012
213
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 19.18 KB | None | 0 0
raw download clone embed print report
  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. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!