tmk_core/protocol/usb_hid/USB_Host_Shield_2.0/PS3USB.cpp

   1 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
   2
   3  This software may be distributed and modified under the terms of the GNU
   4  General Public License version 2 (GPL2) as published by the Free Software
   5  Foundation and appearing in the file GPL2.TXT included in the packaging of
   6  this file. Please note that GPL2 Section 2[b] requires that all works based
   7  on this software must also be made publicly available under the terms of
   8  the GPL2 ("Copyleft").
   9
  10  Contact information
  11  -------------------
  12
  13  Kristian Lauszus, TKJ Electronics
  14  Web      :  http://www.tkjelectronics.com
  15  e-mail   :  kristianl@tkjelectronics.com
  16  */
  17
  18 #include "PS3USB.h"
  19 // To enable serial debugging see "settings.h"
  20 //#define EXTRADEBUG // Uncomment to get even more debugging data
  21 //#define PRINTREPORT // Uncomment to print the report send by the PS3 Controllers
  22
  23 PS3USB::PS3USB(USB *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0) :
  24 pUsb(p), // pointer to USB class instance - mandatory
  25 bAddress(0), // device address - mandatory
  26 bPollEnable(false) // don't start polling before dongle is connected
  27 {
  28         for(uint8_t i = 0; i < PS3_MAX_ENDPOINTS; i++) {
  29                 epInfo[i].epAddr = 0;
  30                 epInfo[i].maxPktSize = (i) ? 0 : 8;
  31                 epInfo[i].epAttribs = 0;
  32                 epInfo[i].bmNakPower = (i) ? USB_NAK_NOWAIT : USB_NAK_MAX_POWER;
  33         }
  34
  35         if(pUsb) // register in USB subsystem
  36                 pUsb->RegisterDeviceClass(this); //set devConfig[] entry
  37
  38         my_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead
  39         my_bdaddr[4] = btadr4;
  40         my_bdaddr[3] = btadr3;
  41         my_bdaddr[2] = btadr2;
  42         my_bdaddr[1] = btadr1;
  43         my_bdaddr[0] = btadr0;
  44 }
  45
  46 uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
  47         uint8_t buf[sizeof (USB_DEVICE_DESCRIPTOR)];
  48         USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
  49         uint8_t rcode;
  50         UsbDevice *p = NULL;
  51         EpInfo *oldep_ptr = NULL;
  52         uint16_t PID;
  53         uint16_t VID;
  54
  55         // get memory address of USB device address pool
  56         AddressPool &addrPool = pUsb->GetAddressPool();
  57 #ifdef EXTRADEBUG
  58         Notify(PSTR("\r\nPS3USB Init"), 0x80);
  59 #endif
  60         // check if address has already been assigned to an instance
  61         if(bAddress) {
  62 #ifdef DEBUG_USB_HOST
  63                 Notify(PSTR("\r\nAddress in use"), 0x80);
  64 #endif
  65                 return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
  66         }
  67
  68         // Get pointer to pseudo device with address 0 assigned
  69         p = addrPool.GetUsbDevicePtr(0);
  70
  71         if(!p) {
  72 #ifdef DEBUG_USB_HOST
  73                 Notify(PSTR("\r\nAddress not found"), 0x80);
  74 #endif
  75                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
  76         }
  77
  78         if(!p->epinfo) {
  79 #ifdef DEBUG_USB_HOST
  80                 Notify(PSTR("\r\nepinfo is null"), 0x80);
  81 #endif
  82                 return USB_ERROR_EPINFO_IS_NULL;
  83         }
  84
  85         // Save old pointer to EP_RECORD of address 0
  86         oldep_ptr = p->epinfo;
  87
  88         // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence
  89         p->epinfo = epInfo;
  90
  91         p->lowspeed = lowspeed;
  92
  93         // Get device descriptor
  94         rcode = pUsb->getDevDescr(0, 0, sizeof (USB_DEVICE_DESCRIPTOR), (uint8_t*)buf); // Get device descriptor - addr, ep, nbytes, data
  95         // Restore p->epinfo
  96         p->epinfo = oldep_ptr;
  97
  98         if(rcode)
  99                 goto FailGetDevDescr;
 100
 101         VID = udd->idVendor;
 102         PID = udd->idProduct;
 103
 104         if(VID != PS3_VID || (PID != PS3_PID && PID != PS3NAVIGATION_PID && PID != PS3MOVE_PID))
 105                 goto FailUnknownDevice;
 106
 107         // Allocate new address according to device class
 108         bAddress = addrPool.AllocAddress(parent, false, port);
 109
 110         if(!bAddress)
 111                 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
 112
 113         // Extract Max Packet Size from device descriptor
 114         epInfo[0].maxPktSize = udd->bMaxPacketSize0;
 115
 116         // Assign new address to the device
 117         rcode = pUsb->setAddr(0, 0, bAddress);
 118         if(rcode) {
 119                 p->lowspeed = false;
 120                 addrPool.FreeAddress(bAddress);
 121                 bAddress = 0;
 122 #ifdef DEBUG_USB_HOST
 123                 Notify(PSTR("\r\nsetAddr: "), 0x80);
 124                 D_PrintHex<uint8_t > (rcode, 0x80);
 125 #endif
 126                 return rcode;
 127         }
 128 #ifdef EXTRADEBUG
 129         Notify(PSTR("\r\nAddr: "), 0x80);
 130         D_PrintHex<uint8_t > (bAddress, 0x80);
 131 #endif
 132         //delay(300); // Spec says you should wait at least 200ms
 133
 134         p->lowspeed = false;
 135
 136         //get pointer to assigned address record
 137         p = addrPool.GetUsbDevicePtr(bAddress);
 138         if(!p)
 139                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 140
 141         p->lowspeed = lowspeed;
 142
 143         // Assign epInfo to epinfo pointer - only EP0 is known
 144         rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
 145         if(rcode)
 146                 goto FailSetDevTblEntry;
 147
 148
 149         /* The application will work in reduced host mode, so we can save program and data
 150            memory space. After verifying the PID and VID we will use known values for the
 151            configuration values for device, interface, endpoints and HID for the PS3 Controllers */
 152
 153         /* Initialize data structures for endpoints of device */
 154         epInfo[ PS3_OUTPUT_PIPE ].epAddr = 0x02; // PS3 output endpoint
 155         epInfo[ PS3_OUTPUT_PIPE ].epAttribs = USB_TRANSFER_TYPE_INTERRUPT;
 156         epInfo[ PS3_OUTPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
 157         epInfo[ PS3_OUTPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE;
 158         epInfo[ PS3_OUTPUT_PIPE ].bmSndToggle = 0;
 159         epInfo[ PS3_OUTPUT_PIPE ].bmRcvToggle = 0;
 160         epInfo[ PS3_INPUT_PIPE ].epAddr = 0x01; // PS3 report endpoint
 161         epInfo[ PS3_INPUT_PIPE ].epAttribs = USB_TRANSFER_TYPE_INTERRUPT;
 162         epInfo[ PS3_INPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
 163         epInfo[ PS3_INPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE;
 164         epInfo[ PS3_INPUT_PIPE ].bmSndToggle = 0;
 165         epInfo[ PS3_INPUT_PIPE ].bmRcvToggle = 0;
 166
 167         rcode = pUsb->setEpInfoEntry(bAddress, 3, epInfo);
 168         if(rcode)
 169                 goto FailSetDevTblEntry;
 170
 171         delay(200); //Give time for address change
 172
 173         rcode = pUsb->setConf(bAddress, epInfo[ PS3_CONTROL_PIPE ].epAddr, 1);
 174         if(rcode)
 175                 goto FailSetConfDescr;
 176
 177         if(PID == PS3_PID || PID == PS3NAVIGATION_PID) {
 178                 if(PID == PS3_PID) {
 179 #ifdef DEBUG_USB_HOST
 180                         Notify(PSTR("\r\nDualshock 3 Controller Connected"), 0x80);
 181 #endif
 182                         PS3Connected = true;
 183                 } else { // must be a navigation controller
 184 #ifdef DEBUG_USB_HOST
 185                         Notify(PSTR("\r\nNavigation Controller Connected"), 0x80);
 186 #endif
 187                         PS3NavigationConnected = true;
 188                 }
 189                 enable_sixaxis(); // The PS3 controller needs a special command before it starts sending data
 190
 191                 // Needed for PS3 Dualshock and Navigation commands to work
 192                 for(uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++)
 193                         writeBuf[i] = pgm_read_byte(&PS3_REPORT_BUFFER[i]);
 194
 195                 for(uint8_t i = 6; i < 10; i++)
 196                         readBuf[i] = 0x7F; // Set the analog joystick values to center position
 197         } else { // must be a Motion controller
 198 #ifdef DEBUG_USB_HOST
 199                 Notify(PSTR("\r\nMotion Controller Connected"), 0x80);
 200 #endif
 201                 PS3MoveConnected = true;
 202                 writeBuf[0] = 0x02; // Set report ID, this is needed for Move commands to work
 203         }
 204         if(my_bdaddr[0] != 0x00 || my_bdaddr[1] != 0x00 || my_bdaddr[2] != 0x00 || my_bdaddr[3] != 0x00 || my_bdaddr[4] != 0x00 || my_bdaddr[5] != 0x00) {
 205                 if(PS3MoveConnected)
 206                         setMoveBdaddr(my_bdaddr); // Set internal Bluetooth address
 207                 else
 208                         setBdaddr(my_bdaddr); // Set internal Bluetooth address
 209
 210 #ifdef DEBUG_USB_HOST
 211                 Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80);
 212                 for(int8_t i = 5; i > 0; i--) {
 213                         D_PrintHex<uint8_t > (my_bdaddr[i], 0x80);
 214                         Notify(PSTR(":"), 0x80);
 215                 }
 216                 D_PrintHex<uint8_t > (my_bdaddr[0], 0x80);
 217 #endif
 218         }
 219         onInit();
 220
 221         bPollEnable = true;
 222         Notify(PSTR("\r\n"), 0x80);
 223         timer = millis();
 224         return 0; // Successful configuration
 225
 226         /* Diagnostic messages */
 227 FailGetDevDescr:
 228 #ifdef DEBUG_USB_HOST
 229         NotifyFailGetDevDescr();
 230         goto Fail;
 231 #endif
 232
 233 FailSetDevTblEntry:
 234 #ifdef DEBUG_USB_HOST
 235         NotifyFailSetDevTblEntry();
 236         goto Fail;
 237 #endif
 238
 239 FailSetConfDescr:
 240 #ifdef DEBUG_USB_HOST
 241         NotifyFailSetConfDescr();
 242 #endif
 243         goto Fail;
 244
 245 FailUnknownDevice:
 246 #ifdef DEBUG_USB_HOST
 247         NotifyFailUnknownDevice(VID, PID);
 248 #endif
 249         rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
 250
 251 Fail:
 252 #ifdef DEBUG_USB_HOST
 253         Notify(PSTR("\r\nPS3 Init Failed, error code: "), 0x80);
 254         NotifyFail(rcode);
 255 #endif
 256         Release();
 257         return rcode;
 258 }
 259
 260 /* Performs a cleanup after failed Init() attempt */
 261 uint8_t PS3USB::Release() {
 262         PS3Connected = false;
 263         PS3MoveConnected = false;
 264         PS3NavigationConnected = false;
 265         pUsb->GetAddressPool().FreeAddress(bAddress);
 266         bAddress = 0;
 267         bPollEnable = false;
 268         return 0;
 269 }
 270
 271 uint8_t PS3USB::Poll() {
 272         if(!bPollEnable)
 273                 return 0;
 274
 275         if(PS3Connected || PS3NavigationConnected) {
 276                 uint16_t BUFFER_SIZE = EP_MAXPKTSIZE;
 277                 pUsb->inTransfer(bAddress, epInfo[ PS3_INPUT_PIPE ].epAddr, &BUFFER_SIZE, readBuf); // input on endpoint 1
 278                 if(millis() - timer > 100) { // Loop 100ms before processing data
 279                         readReport();
 280 #ifdef PRINTREPORT
 281                         printReport(); // Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers
 282 #endif
 283                 }
 284         } else if(PS3MoveConnected) { // One can only set the color of the bulb, set the rumble, set and get the bluetooth address and calibrate the magnetometer via USB
 285                 if(millis() - timer > 4000) { // Send at least every 4th second
 286                         Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE); // The Bulb and rumble values, has to be written again and again, for it to stay turned on
 287                         timer = millis();
 288                 }
 289         }
 290         return 0;
 291 }
 292
 293 void PS3USB::readReport() {
 294         ButtonState = (uint32_t)(readBuf[2] | ((uint16_t)readBuf[3] << 8) | ((uint32_t)readBuf[4] << 16));
 295
 296         //Notify(PSTR("\r\nButtonState", 0x80);
 297         //PrintHex<uint32_t>(ButtonState, 0x80);
 298
 299         if(ButtonState != OldButtonState) {
 300                 ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable
 301                 OldButtonState = ButtonState;
 302         }
 303 }
 304
 305 void PS3USB::printReport() { // Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers
 306 #ifdef PRINTREPORT
 307         for(uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++) {
 308                 D_PrintHex<uint8_t > (readBuf[i], 0x80);
 309                 Notify(PSTR(" "), 0x80);
 310         }
 311         Notify(PSTR("\r\n"), 0x80);
 312 #endif
 313 }
 314
 315 bool PS3USB::getButtonPress(ButtonEnum b) {
 316         return (ButtonState & pgm_read_dword(&PS3_BUTTONS[(uint8_t)b]));
 317 }
 318
 319 bool PS3USB::getButtonClick(ButtonEnum b) {
 320         uint32_t button = pgm_read_dword(&PS3_BUTTONS[(uint8_t)b]);
 321         bool click = (ButtonClickState & button);
 322         ButtonClickState &= ~button; // Clear "click" event
 323         return click;
 324 }
 325
 326 uint8_t PS3USB::getAnalogButton(ButtonEnum a) {
 327         return (uint8_t)(readBuf[(pgm_read_byte(&PS3_ANALOG_BUTTONS[(uint8_t)a])) - 9]);
 328 }
 329
 330 uint8_t PS3USB::getAnalogHat(AnalogHatEnum a) {
 331         return (uint8_t)(readBuf[((uint8_t)a + 6)]);
 332 }
 333
 334 uint16_t PS3USB::getSensor(SensorEnum a) {
 335         return ((readBuf[((uint16_t)a) - 9] << 8) | readBuf[((uint16_t)a + 1) - 9]);
 336 }
 337
 338 double PS3USB::getAngle(AngleEnum a) {
 339         if(PS3Connected) {
 340                 double accXval;
 341                 double accYval;
 342                 double accZval;
 343
 344                 // Data for the Kionix KXPC4 used in the DualShock 3
 345                 const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V)
 346                 accXval = -((double)getSensor(aX) - zeroG);
 347                 accYval = -((double)getSensor(aY) - zeroG);
 348                 accZval = -((double)getSensor(aZ) - zeroG);
 349
 350                 // Convert to 360 degrees resolution
 351                 // atan2 outputs the value of -π to π (radians)
 352                 // We are then converting it to 0 to 2π and then to degrees
 353                 if(a == Pitch)
 354                         return (atan2(accYval, accZval) + PI) * RAD_TO_DEG;
 355                 else
 356                         return (atan2(accXval, accZval) + PI) * RAD_TO_DEG;
 357         } else
 358                 return 0;
 359 }
 360
 361 bool PS3USB::getStatus(StatusEnum c) {
 362         return (readBuf[((uint16_t)c >> 8) - 9] == ((uint8_t)c & 0xff));
 363 }
 364
 365 void PS3USB::printStatusString() {
 366         char statusOutput[100]; // Max string length plus null character
 367         if(PS3Connected || PS3NavigationConnected) {
 368                 strcpy_P(statusOutput, PSTR("ConnectionStatus: "));
 369
 370                 if(getStatus(Plugged)) strcat_P(statusOutput, PSTR("Plugged"));
 371                 else if(getStatus(Unplugged)) strcat_P(statusOutput, PSTR("Unplugged"));
 372                 else strcat_P(statusOutput, PSTR("Error"));
 373
 374                 strcat_P(statusOutput, PSTR(" - PowerRating: "));
 375
 376                 if(getStatus(Charging)) strcat_P(statusOutput, PSTR("Charging"));
 377                 else if(getStatus(NotCharging)) strcat_P(statusOutput, PSTR("Not Charging"));
 378                 else if(getStatus(Shutdown)) strcat_P(statusOutput, PSTR("Shutdown"));
 379                 else if(getStatus(Dying)) strcat_P(statusOutput, PSTR("Dying"));
 380                 else if(getStatus(Low)) strcat_P(statusOutput, PSTR("Low"));
 381                 else if(getStatus(High)) strcat_P(statusOutput, PSTR("High"));
 382                 else if(getStatus(Full)) strcat_P(statusOutput, PSTR("Full"));
 383                 else strcat_P(statusOutput, PSTR("Error"));
 384
 385                 strcat_P(statusOutput, PSTR(" - WirelessStatus: "));
 386
 387                 if(getStatus(CableRumble)) strcat_P(statusOutput, PSTR("Cable - Rumble is on"));
 388                 else if(getStatus(Cable)) strcat_P(statusOutput, PSTR("Cable - Rumble is off"));
 389                 else if(getStatus(BluetoothRumble)) strcat_P(statusOutput, PSTR("Bluetooth - Rumble is on"));
 390                 else if(getStatus(Bluetooth)) strcat_P(statusOutput, PSTR("Bluetooth - Rumble is off"));
 391                 else strcat_P(statusOutput, PSTR("Error"));
 392         } else
 393                 strcpy_P(statusOutput, PSTR("Error"));
 394
 395         USB_HOST_SERIAL.write(statusOutput);
 396 }
 397
 398 /* Playstation Sixaxis Dualshock and Navigation Controller commands */
 399 void PS3USB::PS3_Command(uint8_t *data, uint16_t nbytes) {
 400         // bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0x01), Report Type (Output 0x02), interface (0x00), datalength, datalength, data)
 401         pUsb->ctrlReq(bAddress, epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x01, 0x02, 0x00, nbytes, nbytes, data, NULL);
 402 }
 403
 404 void PS3USB::setAllOff() {
 405         for(uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++)
 406                 writeBuf[i] = pgm_read_byte(&PS3_REPORT_BUFFER[i]); // Reset buffer
 407
 408         PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
 409 }
 410
 411 void PS3USB::setRumbleOff() {
 412         writeBuf[1] = 0x00;
 413         writeBuf[2] = 0x00; // Low mode off
 414         writeBuf[3] = 0x00;
 415         writeBuf[4] = 0x00; // High mode off
 416
 417         PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
 418 }
 419
 420 void PS3USB::setRumbleOn(RumbleEnum mode) {
 421         if((mode & 0x30) > 0x00) {
 422                 uint8_t power[2] = {0xff, 0x00}; // Defaults to RumbleLow
 423                 if(mode == RumbleHigh) {
 424                         power[0] = 0x00;
 425                         power[1] = 0xff;
 426                 }
 427                 setRumbleOn(0xfe, power[0], 0xfe, power[1]);
 428         }
 429 }
 430
 431 void PS3USB::setRumbleOn(uint8_t rightDuration, uint8_t rightPower, uint8_t leftDuration, uint8_t leftPower) {
 432         writeBuf[1] = rightDuration;
 433         writeBuf[2] = rightPower;
 434         writeBuf[3] = leftDuration;
 435         writeBuf[4] = leftPower;
 436         PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
 437 }
 438
 439 void PS3USB::setLedRaw(uint8_t value) {
 440         writeBuf[9] = value << 1;
 441         PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
 442 }
 443
 444 void PS3USB::setLedOff(LEDEnum a) {
 445         writeBuf[9] &= ~((uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1));
 446         PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
 447 }
 448
 449 void PS3USB::setLedOn(LEDEnum a) {
 450         if(a == OFF)
 451                 setLedRaw(0);
 452         else {
 453                 writeBuf[9] |= (uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1);
 454                 PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
 455         }
 456 }
 457
 458 void PS3USB::setLedToggle(LEDEnum a) {
 459         writeBuf[9] ^= (uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1);
 460         PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
 461 }
 462
 463 void PS3USB::setBdaddr(uint8_t *bdaddr) {
 464         /* Set the internal Bluetooth address */
 465         uint8_t buf[8];
 466         buf[0] = 0x01;
 467         buf[1] = 0x00;
 468
 469         for(uint8_t i = 0; i < 6; i++)
 470                 buf[i + 2] = bdaddr[5 - i]; // Copy into buffer, has to be written reversed, so it is MSB first
 471
 472         // bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0xF5), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
 473         pUsb->ctrlReq(bAddress, epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL);
 474 }
 475
 476 void PS3USB::getBdaddr(uint8_t *bdaddr) {
 477         uint8_t buf[8];
 478
 479         // bmRequest = Device to host (0x80) | Class (0x20) | Interface (0x01) = 0xA1, bRequest = Get Report (0x01), Report ID (0xF5), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
 480         pUsb->ctrlReq(bAddress, epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_IN, HID_REQUEST_GET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL);
 481
 482         for(uint8_t i = 0; i < 6; i++)
 483                 bdaddr[5 - i] = buf[i + 2]; // Copy into buffer reversed, so it is LSB first
 484 }
 485
 486 void PS3USB::enable_sixaxis() { // Command used to enable the Dualshock 3 and Navigation controller to send data via USB
 487         uint8_t cmd_buf[4];
 488         cmd_buf[0] = 0x42; // Special PS3 Controller enable commands
 489         cmd_buf[1] = 0x0c;
 490         cmd_buf[2] = 0x00;
 491         cmd_buf[3] = 0x00;
 492
 493         // bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0xF4), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data)
 494         pUsb->ctrlReq(bAddress, epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF4, 0x03, 0x00, 4, 4, cmd_buf, NULL);
 495 }
 496
 497 /* Playstation Move Controller commands */
 498 void PS3USB::Move_Command(uint8_t *data, uint16_t nbytes) {
 499         pUsb->outTransfer(bAddress, epInfo[ PS3_OUTPUT_PIPE ].epAddr, nbytes, data);
 500 }
 501
 502 void PS3USB::moveSetBulb(uint8_t r, uint8_t g, uint8_t b) { // Use this to set the Color using RGB values
 503         // Set the Bulb's values into the write buffer
 504         writeBuf[2] = r;
 505         writeBuf[3] = g;
 506         writeBuf[4] = b;
 507
 508         Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE);
 509 }
 510
 511 void PS3USB::moveSetBulb(ColorsEnum color) { // Use this to set the Color using the predefined colors in "enums.h"
 512         moveSetBulb((uint8_t)(color >> 16), (uint8_t)(color >> 8), (uint8_t)(color));
 513 }
 514
 515 void PS3USB::moveSetRumble(uint8_t rumble) {
 516 #ifdef DEBUG_USB_HOST
 517         if(rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100)
 518                 Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"), 0x80);
 519 #endif
 520         writeBuf[6] = rumble; // Set the rumble value into the write buffer
 521
 522         Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE);
 523 }
 524
 525 void PS3USB::setMoveBdaddr(uint8_t *bdaddr) {
 526         /* Set the internal Bluetooth address */
 527         uint8_t buf[11];
 528         buf[0] = 0x05;
 529         buf[7] = 0x10;
 530         buf[8] = 0x01;
 531         buf[9] = 0x02;
 532         buf[10] = 0x12;
 533
 534         for(uint8_t i = 0; i < 6; i++)
 535                 buf[i + 1] = bdaddr[i];
 536
 537         // bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0x05), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
 538         pUsb->ctrlReq(bAddress, epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x05, 0x03, 0x00, 11, 11, buf, NULL);
 539 }
 540
 541 void PS3USB::getMoveBdaddr(uint8_t *bdaddr) {
 542         uint8_t buf[16];
 543
 544         // bmRequest = Device to host (0x80) | Class (0x20) | Interface (0x01) = 0xA1, bRequest = Get Report (0x01), Report ID (0x04), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
 545         pUsb->ctrlReq(bAddress, epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_IN, HID_REQUEST_GET_REPORT, 0x04, 0x03, 0x00, 16, 16, buf, NULL);
 546
 547         for(uint8_t i = 0; i < 6; i++)
 548                 bdaddr[i] = buf[10 + i];
 549 }
 550
 551 void PS3USB::getMoveCalibration(uint8_t *data) {
 552         uint8_t buf[49];
 553
 554         for(uint8_t i = 0; i < 3; i++) {
 555                 // bmRequest = Device to host (0x80) | Class (0x20) | Interface (0x01) = 0xA1, bRequest = Get Report (0x01), Report ID (0x10), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
 556                 pUsb->ctrlReq(bAddress, epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_IN, HID_REQUEST_GET_REPORT, 0x10, 0x03, 0x00, 49, 49, buf, NULL);
 557
 558                 for(byte j = 0; j < 49; j++)
 559                         data[49 * i + j] = buf[j];
 560         }
 561 }
 562
 563 void PS3USB::onInit() {
 564         if(pFuncOnInit)
 565                 pFuncOnInit(); // Call the user function
 566         else {
 567                 if(PS3MoveConnected)
 568                         moveSetBulb(Red);
 569                 else // Dualshock 3 or Navigation controller
 570                         setLedOn(LED1);
 571         }
 572 }