specchioriflesso/USB_Host_Shield_2.0
1
Fork 0
mirror of https://github.com/felis/USB_Host_Shield_2.0.git synced 2024-03-22 11:31:26 +01:00
Code Issues Projects Releases Packages Wiki Activity

port debugging of new files, need to eliminate all serial.print yet.

Browse source
This commit is contained in:
Andrew J. Kroll 2013-03-28 04:37:09 -04:00
parent 629594f957
commit 9b224b9899
7 changed files with 1736 additions and 1713 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2047
BTD.cpp
View file

File diff suppressed because it is too large Load diff

216
PS3BT.cpp
View file

@ -1,15 +1,15 @@
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
@ -21,15 +21,15 @@
//#define PRINTREPORT // Uncomment to print the report send by the PS3 Controllers
const uint8_t OUTPUT_REPORT_BUFFER[] PROGMEM = {
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
0x00, 0x00, 0x00, 0x00, 0x00,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
PS3BT::PS3BT(BTD *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0):
@ -37,27 +37,27 @@ pBtd(p) // pointer to USB class instance - mandatory
{
if (pBtd)
pBtd->registerServiceClass(this); // Register it as a Bluetooth service
pBtd->my_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead
pBtd->my_bdaddr[4] = btadr4;
pBtd->my_bdaddr[3] = btadr3;
pBtd->my_bdaddr[2] = btadr2;
pBtd->my_bdaddr[1] = btadr1;
pBtd->my_bdaddr[0] = btadr0;
HIDBuffer[0] = 0x52;// HID BT Set_report (0x50) | Report Type (Output 0x02)
HIDBuffer[1] = 0x01;// Report ID
//Needed for PS3 Move Controller commands to work via bluetooth
HIDMoveBuffer[0] = 0xA2;// HID BT DATA_request (0xA0) | Report Type (Output 0x02)
HIDMoveBuffer[1] = 0x02;// Report ID
/* Set device cid for the control and intterrupt channelse - LSB */
control_dcid[0] = 0x40;//0x0040
control_dcid[1] = 0x00;
interrupt_dcid[0] = 0x41;//0x0041
interrupt_dcid[1] = 0x00;
Reset();
}
bool PS3BT::getButtonPress(Button b) {
@ -76,7 +76,7 @@ uint8_t PS3BT::getAnalogButton(Button a) {
}
uint8_t PS3BT::getAnalogHat(AnalogHat a) {
if (l2capinbuf == NULL)
return 0;
return 0;
return (uint8_t)(l2capinbuf[(uint8_t)a+15]);
}
int16_t PS3BT::getSensor(Sensor a) {
@ -93,7 +93,7 @@ int16_t PS3BT::getSensor(Sensor a) {
else if (a == mZmove || a == tempMove) // The tempearature is also 12 bits long
return ((l2capinbuf[(uint16_t)a] << 4) | ((l2capinbuf[(uint16_t)a + 1] & 0xF0) >> 4));
else // aXmove, aYmove, aZmove, gXmove, gYmove and gZmove
return (l2capinbuf[(uint16_t)a] | (l2capinbuf[(uint16_t)a + 1] << 8));
return (l2capinbuf[(uint16_t)a] | (l2capinbuf[(uint16_t)a + 1] << 8));
} else
return 0;
}
@ -101,7 +101,7 @@ double PS3BT::getAngle(Angle a) {
double accXval;
double accYval;
double accZval;
if(PS3Connected) {
// Data for the Kionix KXPC4 used in the DualShock 3
const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V)
@ -115,7 +115,7 @@ double PS3BT::getAngle(Angle a) {
accYval = (int16_t)(getSensor(aYmove)-zeroG);
accZval = (int16_t)(getSensor(aZmove)-zeroG);
}
// Convert to 360 degrees resolution
// atan2 outputs the value of -π to π (radians)
// We are then converting it to 0 to 2π and then to degrees
@ -157,15 +157,15 @@ double PS3BT::get9DOFValues(Sensor a) { // Thanks to Manfred Piendl
}
String PS3BT::getTemperature() {
if(PS3MoveConnected) {
int16_t input = getSensor(tempMove);
int16_t input = getSensor(tempMove);
String output = String(input/100);
output += ".";
if(input%100 < 10)
output += "0";
output += String(input%100);
return output;
return output;
}
}
bool PS3BT::getStatus(Status c) {
@ -178,14 +178,14 @@ bool PS3BT::getStatus(Status c) {
String PS3BT::getStatusString() {
if (PS3Connected || PS3NavigationConnected) {
char statusOutput[100];
strcpy(statusOutput,"ConnectionStatus: ");
if (getStatus(Plugged)) strcat(statusOutput,"Plugged");
else if (getStatus(Unplugged)) strcat(statusOutput,"Unplugged");
else strcat(statusOutput,"Error");
strcat(statusOutput," - PowerRating: ");
if (getStatus(Charging)) strcat(statusOutput,"Charging");
else if (getStatus(NotCharging)) strcat(statusOutput,"Not Charging");
@ -195,23 +195,23 @@ String PS3BT::getStatusString() {
else if (getStatus(High)) strcat(statusOutput,"High");
else if (getStatus(Full)) strcat(statusOutput,"Full");
else strcat(statusOutput,"Error");
strcat(statusOutput," - WirelessStatus: ");
if (getStatus(CableRumble)) strcat(statusOutput,"Cable - Rumble is on");
else if (getStatus(Cable)) strcat(statusOutput,"Cable - Rumble is off");
else if (getStatus(BluetoothRumble)) strcat(statusOutput,"Bluetooth - Rumble is on");
else if (getStatus(Bluetooth)) strcat(statusOutput,"Bluetooth - Rumble is off");
else strcat(statusOutput,"Error");
return statusOutput;
}
else if(PS3MoveConnected) {
char statusOutput[50];
strcpy(statusOutput,"PowerRating: ");
if (getStatus(MoveCharging)) strcat(statusOutput,"Charging");
else if (getStatus(MoveNotCharging)) strcat(statusOutput,"Not Charging");
else if (getStatus(MoveShutdown)) strcat(statusOutput,"Shutdown");
@ -220,7 +220,7 @@ String PS3BT::getStatusString() {
else if (getStatus(MoveHigh)) strcat(statusOutput,"High");
else if (getStatus(MoveFull)) strcat(statusOutput,"Full");
else strcat(statusOutput,"Error");
return statusOutput;
}
}
@ -231,10 +231,10 @@ void PS3BT::Reset() {
activeConnection = false;
l2cap_event_flag = 0; // Reset flags
l2cap_state = L2CAP_WAIT;
// Needed for PS3 Dualshock Controller commands to work via bluetooth
for (uint8_t i = 0; i < OUTPUT_REPORT_BUFFER_SIZE; i++)
HIDBuffer[i + 2] = pgm_read_byte(&OUTPUT_REPORT_BUFFER[i]); // First two bytes reserved for report type and ID
HIDBuffer[i + 2] = pgm_read_byte(&OUTPUT_REPORT_BUFFER[i]); // First two bytes reserved for report type and ID
}
void PS3BT::disconnect() { // Use this void to disconnect any of the controllers
@ -256,11 +256,11 @@ void PS3BT::ACLData(uint8_t* ACLData) {
remote_name[i] = pBtd->remote_name[i]; // Store the remote name for the connection
#ifdef DEBUG
if(pBtd->hci_version < 3) { // Check the HCI Version of the Bluetooth dongle
Notify(PSTR("\r\nYour dongle may not support reading the analog buttons, sensors and status\r\nYour HCI Version is: "));
Notify(PSTR("\r\nYour dongle may not support reading the analog buttons, sensors and status\r\nYour HCI Version is: "), 0x80);
Serial.print(pBtd->hci_version);
Notify(PSTR("\r\nBut should be at least 3\r\nThis means that it doesn't support Bluetooth Version 2.0+EDR"));
Notify(PSTR("\r\nBut should be at least 3\r\nThis means that it doesn't support Bluetooth Version 2.0+EDR"), 0x80);
}
#endif
#endif
}
}
}
@ -270,34 +270,34 @@ void PS3BT::ACLData(uint8_t* ACLData) {
if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U
if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
#ifdef DEBUG
Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "));
PrintHex<uint8_t>(l2capinbuf[13]);
Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[13], 0x80);
Serial.print(" ");
PrintHex<uint8_t>(l2capinbuf[12]);
PrintHex<uint8_t>(l2capinbuf[12], 0x80);
Serial.print(" Data: ");
PrintHex<uint8_t>(l2capinbuf[17]);
PrintHex<uint8_t>(l2capinbuf[17], 0x80);
Serial.print(" ");
PrintHex<uint8_t>(l2capinbuf[16]);
Serial.print(" ");
PrintHex<uint8_t>(l2capinbuf[15]);
PrintHex<uint8_t>(l2capinbuf[16], 0x80);
Serial.print(" ");
PrintHex<uint8_t>(l2capinbuf[14]);
PrintHex<uint8_t>(l2capinbuf[15], 0x80);
Serial.print(" ");
PrintHex<uint8_t>(l2capinbuf[14], 0x80);
#endif
}
else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
#ifdef EXTRADEBUG
Notify(PSTR("\r\nL2CAP Connection Request - PSM: "));
PrintHex<uint8_t>(l2capinbuf[13]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[12]);
Notify(PSTR(" SCID: "));
PrintHex<uint8_t>(l2capinbuf[15]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[14]);
Notify(PSTR(" Identifier: "));
PrintHex<uint8_t>(l2capinbuf[9]);
Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[13], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[12], 0x80);
Notify(PSTR(" SCID: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[15], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[14], 0x80);
Notify(PSTR(" Identifier: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[9], 0x80);
#endif
if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
identifier = l2capinbuf[9];
control_scid[0] = l2capinbuf[14];
control_scid[1] = l2capinbuf[15];
@ -325,7 +325,7 @@ void PS3BT::ACLData(uint8_t* ACLData) {
else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
//Serial.print("\r\nHID Control Configuration Request");
identifier = l2capinbuf[9];
identifier = l2capinbuf[9];
l2cap_event_flag |= L2CAP_FLAG_CONFIG_CONTROL_REQUEST;
}
else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
@ -333,11 +333,11 @@ void PS3BT::ACLData(uint8_t* ACLData) {
identifier = l2capinbuf[9];
l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_REQUEST;
}
}
}
else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
#ifdef DEBUG
Notify(PSTR("\r\nDisconnect Request: Control Channel"));
Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80);
#endif
identifier = l2capinbuf[9];
pBtd->l2cap_disconnection_response(hci_handle,identifier,control_dcid,control_scid);
@ -345,7 +345,7 @@ void PS3BT::ACLData(uint8_t* ACLData) {
}
else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
#ifdef DEBUG
Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"));
Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80);
#endif
identifier = l2capinbuf[9];
pBtd->l2cap_disconnection_response(hci_handle,identifier,interrupt_dcid,interrupt_scid);
@ -353,12 +353,12 @@ void PS3BT::ACLData(uint8_t* ACLData) {
}
}
else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
if (l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
if (l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
//Serial.print("\r\nDisconnect Response: Control Channel");
identifier = l2capinbuf[9];
l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE;
}
else if (l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) {
else if (l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) {
//Serial.print("\r\nDisconnect Response: Interrupt Channel");
identifier = l2capinbuf[9];
l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE;
@ -366,8 +366,8 @@ void PS3BT::ACLData(uint8_t* ACLData) {
}
#ifdef EXTRADEBUG
else {
Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "));
PrintHex<uint8_t>(l2capinbuf[8]);
Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[8], 0x80);
}
#endif
} else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
@ -379,18 +379,18 @@ void PS3BT::ACLData(uint8_t* ACLData) {
ButtonState = (uint32_t)(l2capinbuf[11] | ((uint16_t)l2capinbuf[12] << 8) | ((uint32_t)l2capinbuf[13] << 16));
else if(PS3MoveConnected)
ButtonState = (uint32_t)(l2capinbuf[10] | ((uint16_t)l2capinbuf[11] << 8) | ((uint32_t)l2capinbuf[12] << 16));
//Notify(PSTR("\r\nButtonState");
//PrintHex<uint32_t>(ButtonState);
//Notify(PSTR("\r\nButtonState", 0x80);
//PrintHex<uint32_t>(ButtonState, 0x80);
if(ButtonState != OldButtonState) {
ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable
OldButtonState = ButtonState;
}
#ifdef PRINTREPORT // Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers
for(uint8_t i = 10; i < 58;i++) {
PrintHex<uint8_t>(l2capinbuf[i]);
PrintHex<uint8_t>(l2capinbuf[i], 0x80);
Serial.print(" ");
}
Serial.println();
@ -406,7 +406,7 @@ void PS3BT::L2CAP_task() {
case L2CAP_WAIT:
if (l2cap_connection_request_control_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nHID Control Incoming Connection Request"));
Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80);
#endif
pBtd->l2cap_connection_response(hci_handle,identifier, control_dcid, control_scid, PENDING);
delay(1);
@ -420,17 +420,17 @@ void PS3BT::L2CAP_task() {
case L2CAP_CONTROL_REQUEST:
if (l2cap_config_request_control_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nHID Control Configuration Request"));
Notify(PSTR("\r\nHID Control Configuration Request"), 0x80);
#endif
pBtd->l2cap_config_response(hci_handle,identifier, control_scid);
l2cap_state = L2CAP_CONTROL_SUCCESS;
}
break;
case L2CAP_CONTROL_SUCCESS:
if (l2cap_config_success_control_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nHID Control Successfully Configured"));
Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80);
#endif
l2cap_state = L2CAP_INTERRUPT_SETUP;
}
@ -438,7 +438,7 @@ void PS3BT::L2CAP_task() {
case L2CAP_INTERRUPT_SETUP:
if (l2cap_connection_request_interrupt_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"));
Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80);
#endif
pBtd->l2cap_connection_response(hci_handle,identifier, interrupt_dcid, interrupt_scid, PENDING);
delay(1);
@ -446,14 +446,14 @@ void PS3BT::L2CAP_task() {
identifier++;
delay(1);
pBtd->l2cap_config_request(hci_handle,identifier, interrupt_scid);
l2cap_state = L2CAP_INTERRUPT_REQUEST;
}
break;
case L2CAP_INTERRUPT_REQUEST:
if (l2cap_config_request_interrupt_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nHID Interrupt Configuration Request"));
Notify(PSTR("\r\nHID Interrupt Configuration Request"), 0x80);
#endif
pBtd->l2cap_config_response(hci_handle,identifier, interrupt_scid);
l2cap_state = L2CAP_INTERRUPT_SUCCESS;
@ -462,14 +462,14 @@ void PS3BT::L2CAP_task() {
case L2CAP_INTERRUPT_SUCCESS:
if (l2cap_config_success_interrupt_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nHID Interrupt Successfully Configured"));
Notify(PSTR("\r\nHID Interrupt Successfully Configured"), 0x80);
#endif
if(remote_name[0] == 'M') { // First letter in Motion Controller ('M')
for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) // Reset l2cap in buffer as it sometimes read it as a button has been pressed
l2capinbuf[i] = 0;
ButtonState = 0;
OldButtonState = 0;
l2cap_state = L2CAP_HID_PS3_LED;
} else
l2cap_state = L2CAP_HID_ENABLE_SIXAXIS;
@ -478,22 +478,22 @@ void PS3BT::L2CAP_task() {
break;
/* These states are handled in Run() */
case L2CAP_INTERRUPT_DISCONNECT:
if (l2cap_disconnect_response_interrupt_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nDisconnected Interrupt Channel"));
Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80);
#endif
identifier++;
pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid);
pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid);
l2cap_state = L2CAP_CONTROL_DISCONNECT;
}
break;
case L2CAP_CONTROL_DISCONNECT:
if (l2cap_disconnect_response_control_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nDisconnected Control Channel"));
Notify(PSTR("\r\nDisconnected Control Channel"), 0x80);
#endif
pBtd->hci_disconnect(hci_handle);
hci_handle = -1; // Reset handle
@ -501,7 +501,7 @@ void PS3BT::L2CAP_task() {
l2cap_state = L2CAP_WAIT;
}
break;
}
}
}
void PS3BT::Run() {
switch (l2cap_state) {
@ -511,7 +511,7 @@ void PS3BT::Run() {
l2capinbuf[i] = 0;
ButtonState = 0;
OldButtonState = 0;
enable_sixaxis();
for (uint8_t i = 15; i < 19; i++)
l2capinbuf[i] = 0x7F; // Set the analog joystick values to center position
@ -519,33 +519,33 @@ void PS3BT::Run() {
timer = millis();
}
break;
case L2CAP_HID_PS3_LED:
if(millis() - timer > 1000) { // loop 1 second before sending the command
if (remote_name[0] == 'P') { // First letter in PLAYSTATION(R)3 Controller ('P')
setLedOn(LED1);
#ifdef DEBUG
Notify(PSTR("\r\nDualshock 3 Controller Enabled\r\n"));
Notify(PSTR("\r\nDualshock 3 Controller Enabled\r\n"), 0x80);
#endif
PS3Connected = true;
} else if (remote_name[0] == 'N') { // First letter in Navigation Controller ('N')
setLedOn(LED1); // This just turns LED constantly on, on the Navigation controller
#ifdef DEBUG
Notify(PSTR("\r\nNavigation Controller Enabled\r\n"));
Notify(PSTR("\r\nNavigation Controller Enabled\r\n"), 0x80);
#endif
PS3NavigationConnected = true;
} else if(remote_name[0] == 'M') { // First letter in Motion Controller ('M')
moveSetBulb(Red);
timerBulbRumble = millis();
#ifdef DEBUG
Notify(PSTR("\r\nMotion Controller Enabled\r\n"));
Notify(PSTR("\r\nMotion Controller Enabled\r\n"), 0x80);
#endif
PS3MoveConnected = true;
}
l2cap_state = L2CAP_DONE;
}
break;
case L2CAP_DONE:
if (PS3MoveConnected) { //The Bulb and rumble values, has to be send at aproximatly every 5th second for it to stay on
if (millis() - timerBulbRumble > 4000) { //Send at least every 4th second
@ -562,8 +562,8 @@ void PS3BT::Run() {
/************************************************************/
//Playstation Sixaxis Dualshock and Navigation Controller commands
void PS3BT::HID_Command(uint8_t* data, uint8_t nbytes) {
if (millis() - timerHID <= 250)// Check if is has been more than 250ms since last command
void PS3BT::HID_Command(uint8_t* data, uint8_t nbytes) {
if (millis() - timerHID <= 250)// Check if is has been more than 250ms since last command
delay((uint32_t)(250 - (millis() - timerHID)));//There have to be a delay between commands
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
timerHID = millis();
@ -571,7 +571,7 @@ void PS3BT::HID_Command(uint8_t* data, uint8_t nbytes) {
void PS3BT::setAllOff() {
for (uint8_t i = 0; i < OUTPUT_REPORT_BUFFER_SIZE; i++)
HIDBuffer[i + 2] = pgm_read_byte(&OUTPUT_REPORT_BUFFER[i]);//First two bytes reserved for report type and ID
HID_Command(HIDBuffer, HID_BUFFERSIZE);
}
void PS3BT::setRumbleOff() {
@ -579,7 +579,7 @@ void PS3BT::setRumbleOff() {
HIDBuffer[4] = 0x00;//low mode off
HIDBuffer[5] = 0x00;
HIDBuffer[6] = 0x00;//high mode off
HID_Command(HIDBuffer, HID_BUFFERSIZE);
}
void PS3BT::setRumbleOn(Rumble mode) {
@ -591,7 +591,7 @@ void PS3BT::setRumbleOn(Rumble mode) {
*/
if ((mode & 0x30) > 0) {
HIDBuffer[3] = 0xfe;
HIDBuffer[5] = 0xfe;
HIDBuffer[5] = 0xfe;
if (mode == RumbleHigh) {
HIDBuffer[4] = 0;//low mode off
HIDBuffer[6] = 0xff;//high mode on
@ -599,7 +599,7 @@ void PS3BT::setRumbleOn(Rumble mode) {
else {
HIDBuffer[4] = 0xff;//low mode on
HIDBuffer[6] = 0;//high mode off
}
}
HID_Command(HIDBuffer, HID_BUFFERSIZE);
}
}
@ -623,7 +623,7 @@ void PS3BT::enable_sixaxis() { //Command used to enable the Dualshock 3 and Navi
cmd_buf[3] = 0x03;
cmd_buf[4] = 0x00;
cmd_buf[5] = 0x00;
HID_Command(cmd_buf, 6);
}
@ -635,12 +635,12 @@ void PS3BT::HIDMove_Command(uint8_t* data,uint8_t nbytes) {
timerHID = millis();
}
void PS3BT::moveSetBulb(uint8_t r, uint8_t g, uint8_t b) { //Use this to set the Color using RGB values
//set the Bulb's values into the write buffer
//set the Bulb's values into the write buffer
HIDMoveBuffer[3] = r;
HIDMoveBuffer[4] = g;
HIDMoveBuffer[5] = b;
HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE);
HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE);
}
void PS3BT::moveSetBulb(Colors color) { //Use this to set the Color using the predefined colors in enum
moveSetBulb((uint8_t)(color >> 16),(uint8_t)(color >> 8),(uint8_t)(color));
@ -648,10 +648,10 @@ void PS3BT::moveSetBulb(Colors color) { //Use this to set the Color using the pr
void PS3BT::moveSetRumble(uint8_t rumble) {
#ifdef DEBUG
if(rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100)
Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"));
Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"), 0x80);
#endif
//set the rumble value into the write buffer
HIDMoveBuffer[7] = rumble;
HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE);
HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE);
}

270
PS3USB.cpp
View file

@ -1,15 +1,15 @@
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
@ -21,20 +21,20 @@
//#define PRINTREPORT // Uncomment to print the report send by the PS3 Controllers
const uint8_t PS3_REPORT_BUFFER[] PROGMEM = {
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
0x00, 0x00, 0x00, 0x00, 0x00,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
const uint8_t MOVE_REPORT_BUFFER[] PROGMEM = {
0x02, 0x00, // Always 0x02, 0x00,
0x00, 0x00, 0x00, // r, g, b,
0x00, // Always 0x00,
0x02, 0x00, // Always 0x02, 0x00,
0x00, 0x00, 0x00, // r, g, b,
0x00, // Always 0x00,
0x00 // Rumble
};
@ -46,13 +46,13 @@ bPollEnable(false) // don't start polling before dongle is connected
for(uint8_t i=0; i<PS3_MAX_ENDPOINTS; i++) {
epInfo[i].epAddr = 0;
epInfo[i].maxPktSize = (i) ? 0 : 8;
epInfo[i].epAttribs = 0;
epInfo[i].epAttribs = 0;
epInfo[i].bmNakPower = (i) ? USB_NAK_NOWAIT : USB_NAK_MAX_POWER;
}
if (pUsb) // register in USB subsystem
pUsb->RegisterDeviceClass(this); //set devConfig[] entry
my_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead
my_bdaddr[4] = btadr4;
my_bdaddr[3] = btadr3;
@ -68,68 +68,68 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
EpInfo *oldep_ptr = NULL;
uint16_t PID;
uint16_t VID;
// get memory address of USB device address pool
AddressPool &addrPool = pUsb->GetAddressPool();
AddressPool &addrPool = pUsb->GetAddressPool();
#ifdef EXTRADEBUG
Notify(PSTR("\r\nPS3USB Init"));
Notify(PSTR("\r\nPS3USB Init"), 0x80);
#endif
// check if address has already been assigned to an instance
if (bAddress) {
#ifdef DEBUG
Notify(PSTR("\r\nAddress in use"));
Notify(PSTR("\r\nAddress in use"), 0x80);
#endif
return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
}
// Get pointer to pseudo device with address 0 assigned
p = addrPool.GetUsbDevicePtr(0);
if (!p) {
if (!p) {
#ifdef DEBUG
Notify(PSTR("\r\nAddress not found"));
Notify(PSTR("\r\nAddress not found"), 0x80);
#endif
return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
}
if (!p->epinfo) {
#ifdef DEBUG
Notify(PSTR("\r\nepinfo is null"));
Notify(PSTR("\r\nepinfo is null"), 0x80);
#endif
return USB_ERROR_EPINFO_IS_NULL;
}
// Save old pointer to EP_RECORD of address 0
oldep_ptr = p->epinfo;
// Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence
p->epinfo = epInfo;
p->lowspeed = lowspeed;
// Get device descriptor
rcode = pUsb->getDevDescr(0, 0, sizeof(USB_DEVICE_DESCRIPTOR), (uint8_t*)buf);// Get device descriptor - addr, ep, nbytes, data
// Restore p->epinfo
p->epinfo = oldep_ptr;
if(rcode)
goto FailGetDevDescr;
VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor;
PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct;
if(VID != PS3_VID || (PID != PS3_PID && PID != PS3NAVIGATION_PID && PID != PS3MOVE_PID))
goto FailUnknownDevice;
goto FailUnknownDevice;
// Allocate new address according to device class
bAddress = addrPool.AllocAddress(parent, false, port);
if (!bAddress)
return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
// Extract Max Packet Size from device descriptor
epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
// Assign new address to the device
rcode = pUsb->setAddr( 0, 0, bAddress );
if (rcode) {
@ -137,34 +137,34 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
addrPool.FreeAddress(bAddress);
bAddress = 0;
#ifdef DEBUG
Notify(PSTR("\r\nsetAddr: "));
Notify(PSTR("\r\nsetAddr: "), 0x80);
#endif
PrintHex<uint8_t>(rcode);
PrintHex<uint8_t>(rcode, 0x80);
return rcode;
}
#ifdef EXTRADEBUG
Notify(PSTR("\r\nAddr: "));
PrintHex<uint8_t>(bAddress);
Notify(PSTR("\r\nAddr: "), 0x80);
PrintHex<uint8_t>(bAddress, 0x80);
#endif
p->lowspeed = false;
//get pointer to assigned address record
p = addrPool.GetUsbDevicePtr(bAddress);
if (!p)
if (!p)
return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
p->lowspeed = lowspeed;
p->lowspeed = lowspeed;
// Assign epInfo to epinfo pointer - only EP0 is known
rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
if (rcode)
goto FailSetDevTblEntry;
/* The application will work in reduced host mode, so we can save program and data
memory space. After verifying the PID and VID we will use known values for the
configuration values for device, interface, endpoints and HID for the PS3 Controllers */
/* Initialize data structures for endpoints of device */
epInfo[ PS3_OUTPUT_PIPE ].epAddr = 0x02; // PS3 output endpoint
epInfo[ PS3_OUTPUT_PIPE ].epAttribs = EP_INTERRUPT;
@ -178,26 +178,26 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
epInfo[ PS3_INPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE;
epInfo[ PS3_INPUT_PIPE ].bmSndToggle = bmSNDTOG0;
epInfo[ PS3_INPUT_PIPE ].bmRcvToggle = bmRCVTOG0;
rcode = pUsb->setEpInfoEntry(bAddress, 3, epInfo);
if( rcode )
goto FailSetDevTblEntry;
delay(200);//Give time for address change
rcode = pUsb->setConf(bAddress, epInfo[ PS3_CONTROL_PIPE ].epAddr, 1);
if( rcode )
goto FailSetConf;
if(PID == PS3_PID || PID == PS3NAVIGATION_PID) {
if(PID == PS3_PID) {
#ifdef DEBUG
Notify(PSTR("\r\nDualshock 3 Controller Connected"));
#endif
Notify(PSTR("\r\nDualshock 3 Controller Connected"), 0x80);
#endif
PS3Connected = true;
} else { // must be a navigation controller
#ifdef DEBUG
Notify(PSTR("\r\nNavigation Controller Connected"));
Notify(PSTR("\r\nNavigation Controller Connected"), 0x80);
#endif
PS3NavigationConnected = true;
}
@ -205,62 +205,62 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
setBdaddr(my_bdaddr);
enable_sixaxis();
setLedOn(LED1);
// Needed for PS3 Dualshock and Navigation commands to work
for (uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++)
writeBuf[i] = pgm_read_byte(&PS3_REPORT_BUFFER[i]);
for (uint8_t i = 6; i < 10; i++)
readBuf[i] = 0x7F; // Set the analog joystick values to center position
}
else { // must be a Motion controller
#ifdef DEBUG
Notify(PSTR("\r\nMotion Controller Connected"));
#endif
Notify(PSTR("\r\nMotion Controller Connected"), 0x80);
#endif
PS3MoveConnected = true;
setMoveBdaddr(my_bdaddr); // Set internal bluetooth address
moveSetBulb(Red);
// Needed for Move commands to work
for (uint8_t i = 0; i < MOVE_REPORT_BUFFER_SIZE; i++)
writeBuf[i] = pgm_read_byte(&MOVE_REPORT_BUFFER[i]);
}
bPollEnable = true;
Notify(PSTR("\r\n"));
Notify(PSTR("\r\n"), 0x80);
timer = millis();
return 0; // successful configuration
/* diagnostic messages */
/* diagnostic messages */
FailGetDevDescr:
#ifdef DEBUG
Notify(PSTR("\r\ngetDevDescr:"));
Notify(PSTR("\r\ngetDevDescr:"), 0x80);
#endif
goto Fail;
goto Fail;
FailSetDevTblEntry:
#ifdef DEBUG
Notify(PSTR("\r\nsetDevTblEn:"));
Notify(PSTR("\r\nsetDevTblEn:"), 0x80);
#endif
goto Fail;
FailSetConf:
#ifdef DEBUG
Notify(PSTR("\r\nsetConf:"));
Notify(PSTR("\r\nsetConf:"), 0x80);
#endif
goto Fail;
goto Fail;
FailUnknownDevice:
#ifdef DEBUG
Notify(PSTR("\r\nUnknown Device Connected - VID: "));
PrintHex<uint16_t>(VID);
Notify(PSTR(" PID: "));
PrintHex<uint16_t>(PID);
Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80);
PrintHex<uint16_t>(VID, 0x80);
Notify(PSTR(" PID: "), 0x80);
PrintHex<uint16_t>(PID, 0x80);
#endif
rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
goto Fail;
Fail:
#ifdef DEBUG
Notify(PSTR("\r\nPS3 Init Failed, error code: "));
Notify(PSTR("\r\nPS3 Init Failed, error code: "), 0x80);
Serial.print(rcode,HEX);
#endif
#endif
Release();
return rcode;
}
@ -270,15 +270,15 @@ uint8_t PS3USB::Release() {
PS3Connected = false;
PS3MoveConnected = false;
PS3NavigationConnected = false;
pUsb->GetAddressPool().FreeAddress(bAddress);
pUsb->GetAddressPool().FreeAddress(bAddress);
bAddress = 0;
bPollEnable = false;
return 0;
}
uint8_t PS3USB::Poll() {
uint8_t PS3USB::Poll() {
if (!bPollEnable)
return 0;
if(PS3Connected || PS3NavigationConnected) {
uint16_t BUFFER_SIZE = EP_MAXPKTSIZE;
pUsb->inTransfer(bAddress, epInfo[ PS3_INPUT_PIPE ].epAddr, &BUFFER_SIZE, readBuf); // input on endpoint 1
@ -289,25 +289,25 @@ uint8_t PS3USB::Poll() {
#endif
}
}
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
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
if (millis() - timer > 4000) // Send at least every 4th second
{
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
timer = millis();
}
}
}
}
return 0;
}
void PS3USB::readReport() {
void PS3USB::readReport() {
if (readBuf == NULL)
return;
return;
ButtonState = (uint32_t)(readBuf[2] | ((uint16_t)readBuf[3] << 8) | ((uint32_t)readBuf[4] << 16));
//Notify(PSTR("\r\nButtonState");
//PrintHex<uint32_t>(ButtonState);
//Notify(PSTR("\r\nButtonState", 0x80);
//PrintHex<uint32_t>(ButtonState, 0x80);
if(ButtonState != OldButtonState) {
ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable
OldButtonState = ButtonState;
@ -319,9 +319,9 @@ void PS3USB::printReport() { //Uncomment "#define PRINTREPORT" to print the repo
if (readBuf == NULL)
return;
for(uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE;i++) {
PrintHex<uint8_t>(readBuf[i]);
PrintHex<uint8_t>(readBuf[i], 0x80);
Serial.print(" ");
}
}
Serial.println();
#endif
}
@ -342,7 +342,7 @@ uint8_t PS3USB::getAnalogButton(Button a) {
}
uint8_t PS3USB::getAnalogHat(AnalogHat a) {
if (readBuf == NULL)
return 0;
return 0;
return (uint8_t)(readBuf[((uint8_t)a+6)]);
}
uint16_t PS3USB::getSensor(Sensor a) {
@ -350,22 +350,22 @@ uint16_t PS3USB::getSensor(Sensor a) {
return 0;
return ((readBuf[((uint16_t)a)-9] << 8) | readBuf[((uint16_t)a + 1)-9]);
}
double PS3USB::getAngle(Angle a) {
double PS3USB::getAngle(Angle a) {
if(PS3Connected) {
double accXval;
double accYval;
double accZval;
// Data for the Kionix KXPC4 used in the DualShock 3
const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V)
accXval = -((double)getSensor(aX)-zeroG);
accYval = -((double)getSensor(aY)-zeroG);
accZval = -((double)getSensor(aZ)-zeroG);
// Convert to 360 degrees resolution
// atan2 outputs the value of -π to π (radians)
// We are then converting it to 0 to 2π and then to degrees
if (a == Pitch) {
// We are then converting it to 0 to 2π and then to degrees
if (a == Pitch) {
double angle = (atan2(accYval,accZval)+PI)*RAD_TO_DEG;
return angle;
} else {
@ -373,7 +373,7 @@ double PS3USB::getAngle(Angle a) {
return angle;
}
} else
return 0;
return 0;
}
bool PS3USB::getStatus(Status c) {
if (readBuf == NULL)
@ -385,16 +385,16 @@ bool PS3USB::getStatus(Status c) {
String PS3USB::getStatusString() {
if (PS3Connected || PS3NavigationConnected) {
char statusOutput[100];
strcpy(statusOutput,"ConnectionStatus: ");
if (getStatus(Plugged)) strcat(statusOutput,"Plugged");
else if (getStatus(Unplugged)) strcat(statusOutput,"Unplugged");
else strcat(statusOutput,"Error");
strcat(statusOutput," - PowerRating: ");
if (getStatus(Charging)) strcat(statusOutput,"Charging");
else if (getStatus(NotCharging)) strcat(statusOutput,"Not Charging");
else if (getStatus(Shutdown)) strcat(statusOutput,"Shutdown");
@ -403,16 +403,16 @@ String PS3USB::getStatusString() {
else if (getStatus(High)) strcat(statusOutput,"High");
else if (getStatus(Full)) strcat(statusOutput,"Full");
else strcat(statusOutput,"Error");
strcat(statusOutput," - WirelessStatus: ");
if (getStatus(CableRumble)) strcat(statusOutput,"Cable - Rumble is on");
else if (getStatus(Cable)) strcat(statusOutput,"Cable - Rumble is off");
else if (getStatus(BluetoothRumble)) strcat(statusOutput,"Bluetooth - Rumble is on");
else if (getStatus(Bluetooth)) strcat(statusOutput,"Bluetooth - Rumble is off");
else strcat(statusOutput,"Error");
return statusOutput;
return statusOutput;
}
}
@ -424,7 +424,7 @@ void PS3USB::PS3_Command(uint8_t* data, uint16_t nbytes) {
void PS3USB::setAllOff() {
for (uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++)
writeBuf[i] = pgm_read_byte(&PS3_REPORT_BUFFER[i]); // Reset buffer
PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
}
void PS3USB::setRumbleOff() {
@ -432,7 +432,7 @@ void PS3USB::setRumbleOff() {
writeBuf[2] = 0x00;//low mode off
writeBuf[3] = 0x00;
writeBuf[4] = 0x00;//high mode off
PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
}
void PS3USB::setRumbleOn(Rumble mode) {
@ -444,7 +444,7 @@ void PS3USB::setRumbleOn(Rumble mode) {
*/
if ((mode & 0x30) > 0) {
writeBuf[1] = 0xfe;
writeBuf[3] = 0xfe;
writeBuf[3] = 0xfe;
if (mode == RumbleHigh) {
writeBuf[2] = 0;//low mode off
writeBuf[4] = 0xff;//high mode on
@ -468,23 +468,23 @@ void PS3USB::setLedToggle(LED a) {
writeBuf[9] ^= (uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1);
PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
}
void PS3USB::setBdaddr(uint8_t* BDADDR) {
/* Set the internal bluetooth address */
uint8_t buf[8];
void PS3USB::setBdaddr(uint8_t* BDADDR) {
/* Set the internal bluetooth address */
uint8_t buf[8];
buf[0] = 0x01;
buf[1] = 0x00;
for (uint8_t i = 0; i < 6; i++)
buf[i+2] = BDADDR[5 - i];//Copy into buffer, has to be written reversed
//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)
pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL);
pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL);
#ifdef DEBUG
Notify(PSTR("\r\nBluetooth Address was set to: "));
Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80);
for(int8_t i = 5; i > 0; i--) {
PrintHex<uint8_t>(my_bdaddr[i]);
PrintHex<uint8_t>(my_bdaddr[i], 0x80);
Serial.print(":");
}
PrintHex<uint8_t>(my_bdaddr[0]);
PrintHex<uint8_t>(my_bdaddr[0], 0x80);
#endif
return;
}
@ -494,7 +494,7 @@ void PS3USB::enable_sixaxis() { //Command used to enable the Dualshock 3 and Nav
cmd_buf[1] = 0x0c;
cmd_buf[2] = 0x00;
cmd_buf[3] = 0x00;
//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)
pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF4, 0x03, 0x00, 4, 4, cmd_buf, NULL);
}
@ -502,15 +502,15 @@ void PS3USB::enable_sixaxis() { //Command used to enable the Dualshock 3 and Nav
/* Playstation Move Controller commands */
void PS3USB::Move_Command(uint8_t* data, uint16_t nbytes) {
pUsb->outTransfer(bAddress, epInfo[ PS3_OUTPUT_PIPE ].epAddr, nbytes, data);
}
}
void PS3USB::moveSetBulb(uint8_t r, uint8_t g, uint8_t b) { //Use this to set the Color using RGB values
// set the Bulb's values into the write buffer
// set the Bulb's values into the write buffer
writeBuf[2] = r;
writeBuf[3] = g;
writeBuf[4] = b;
Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE);
Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE);
}
void PS3USB::moveSetBulb(Colors color) { //Use this to set the Color using the predefined colors in "enums.h"
moveSetBulb((uint8_t)(color >> 16),(uint8_t)(color >> 8),(uint8_t)(color));
@ -518,34 +518,34 @@ void PS3USB::moveSetBulb(Colors color) { //Use this to set the Color using the p
void PS3USB::moveSetRumble(uint8_t rumble) {
#ifdef DEBUG
if(rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100)
Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"));
Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"), 0x80);
#endif
//set the rumble value into the write buffer
writeBuf[6] = rumble;
Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE);
Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE);
}
void PS3USB::setMoveBdaddr(uint8_t* BDADDR) {
/* Set the internal bluetooth address */
/* Set the internal bluetooth address */
uint8_t buf[11];
buf[0] = 0x05;
buf[7] = 0x10;
buf[8] = 0x01;
buf[9] = 0x02;
buf[10] = 0x12;
buf[10] = 0x12;
for (uint8_t i = 0; i < 6; i++)
buf[i + 1] = BDADDR[i];
//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)
pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x05, 0x03, 0x00,11,11, buf, NULL);
pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x05, 0x03, 0x00,11,11, buf, NULL);
#ifdef DEBUG
Notify(PSTR("\r\nBluetooth Address was set to: "));
Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80);
for(int8_t i = 5; i > 0; i--) {
PrintHex<uint8_t>(my_bdaddr[i]);
PrintHex<uint8_t>(my_bdaddr[i], 0x80);
Serial.print(":");
}
PrintHex<uint8_t>(my_bdaddr[0]);
PrintHex<uint8_t>(my_bdaddr[0], 0x80);
#endif
return;
}

262
SPP.cpp
View file

@ -1,15 +1,15 @@
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
@ -47,16 +47,16 @@ pBtd(p) // Pointer to BTD class instance - mandatory
{
if (pBtd)
pBtd->registerServiceClass(this); // Register it as a Bluetooth service
pBtd->btdName = name;
pBtd->btdPin = pin;
/* Set device cid for the SDP and RFCOMM channelse */
sdp_dcid[0] = 0x50; // 0x0050
sdp_dcid[1] = 0x00;
rfcomm_dcid[0] = 0x51; // 0x0051
rfcomm_dcid[1] = 0x00;
Reset();
}
void SPP::Reset() {
@ -65,7 +65,7 @@ void SPP::Reset() {
SDPConnected = false;
l2cap_sdp_state = L2CAP_SDP_WAIT;
l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT;
l2cap_event_flag = 0;
l2cap_event_flag = 0;
}
void SPP::disconnect(){
connected = false;
@ -96,31 +96,31 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U
if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
#ifdef DEBUG
Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "));
PrintHex<uint8_t>(l2capinbuf[13]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[12]);
Notify(PSTR(" Data: "));
PrintHex<uint8_t>(l2capinbuf[17]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[16]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[15]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[14]);
Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[13], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[12], 0x80);
Notify(PSTR(" Data: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[17], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[16], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[15], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[14], 0x80);
#endif
} else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
#ifdef EXTRADEBUG
Notify(PSTR("\r\nL2CAP Connection Request - PSM: "));
PrintHex<uint8_t>(l2capinbuf[13]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[12]);
Notify(PSTR(" SCID: "));
PrintHex<uint8_t>(l2capinbuf[15]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[14]);
Notify(PSTR(" Identifier: "));
PrintHex<uint8_t>(l2capinbuf[9]);
Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[13], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[12], 0x80);
Notify(PSTR(" SCID: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[15], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[14], 0x80);
Notify(PSTR(" Identifier: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[9], 0x80);
#endif
if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == SDP_PSM) { // It doesn't matter if it receives another reqeust, since it waits for the channel to disconnect in the L2CAP_SDP_DONE state, and the l2cap_event_flag will be cleared if so
identifier = l2capinbuf[9];
@ -157,11 +157,11 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
}
} else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
if (l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) {
//Notify(PSTR("\r\nDisconnect Request: SDP Channel"));
//Notify(PSTR("\r\nDisconnect Request: SDP Channel"), 0x80);
identifier = l2capinbuf[9];
l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_SDP_REQUEST;
} else if (l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) {
//Notify(PSTR("\r\nDisconnect Request: RFCOMM Channel"));
//Notify(PSTR("\r\nDisconnect Request: RFCOMM Channel"), 0x80);
identifier = l2capinbuf[9];
l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST;
}
@ -177,15 +177,15 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
}
} else if (l2capinbuf[8] == L2CAP_CMD_INFORMATION_REQUEST) {
#ifdef DEBUG
Notify(PSTR("\r\nInformation request"));
Notify(PSTR("\r\nInformation request"), 0x80);
#endif
identifier = l2capinbuf[9];
pBtd->l2cap_information_response(hci_handle,identifier,l2capinbuf[12],l2capinbuf[13]);
}
#ifdef EXTRADEBUG
else {
Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "));
PrintHex<uint8_t>(l2capinbuf[8]);
Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[8], 0x80);
}
#endif
} else if (l2capinbuf[6] == sdp_dcid[0] && l2capinbuf[7] == sdp_dcid[1]) { // SDP
@ -221,25 +221,25 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
rfcommCommandResponse = l2capinbuf[8] & 0x02;
rfcommChannelType = l2capinbuf[9] & 0xEF;
rfcommPfBit = l2capinbuf[9] & 0x10;
if(rfcommChannel>>3 != 0x00)
rfcommChannelConnection = rfcommChannel;
#ifdef EXTRADEBUG
Notify(PSTR("\r\nRFCOMM Channel: "));
Notify(PSTR("\r\nRFCOMM Channel: "), 0x80);
Serial.print(rfcommChannel>>3,HEX);
Notify(PSTR(" Direction: "));
Notify(PSTR(" Direction: "), 0x80);
Serial.print(rfcommDirection>>2,HEX);
Notify(PSTR(" CommandResponse: "));
Notify(PSTR(" CommandResponse: "), 0x80);
Serial.print(rfcommCommandResponse>>1,HEX);
Notify(PSTR(" ChannelType: "));
Notify(PSTR(" ChannelType: "), 0x80);
Serial.print(rfcommChannelType,HEX);
Notify(PSTR(" PF_BIT: "));
Notify(PSTR(" PF_BIT: "), 0x80);
Serial.print(rfcommPfBit,HEX);
#endif
if (rfcommChannelType == RFCOMM_DISC) {
#ifdef DEBUG
Notify(PSTR("\r\nReceived Disconnect RFCOMM Command on channel: "));
Notify(PSTR("\r\nReceived Disconnect RFCOMM Command on channel: "), 0x80);
Serial.print(rfcommChannel>>3,HEX);
#endif
connected = false;
@ -256,10 +256,10 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
rfcommAvailable += length;
}
#ifdef EXTRADEBUG
Notify(PSTR("\r\nRFCOMM Data Available: "));
Notify(PSTR("\r\nRFCOMM Data Available: "), 0x80);
Serial.print(rfcommAvailable);
if (offset) {
Notify(PSTR(" - Credit: 0x"));
Notify(PSTR(" - Credit: 0x"), 0x80);
Serial.print(l2capinbuf[11],HEX);
}
#endif
@ -269,7 +269,7 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
#endif
} else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command
#ifdef DEBUG
Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command"));
Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command"), 0x80);
#endif
rfcommbuf[0] = BT_RFCOMM_RPN_RSP; // Command
rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1
@ -284,7 +284,7 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x0A); // UIH Remote Port Negotiation Response
} else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command
#ifdef DEBUG
Notify(PSTR("\r\nSend UIH Modem Status Response"));
Notify(PSTR("\r\nSend UIH Modem Status Response"), 0x80);
#endif
rfcommbuf[0] = BT_RFCOMM_MSC_RSP; // UIH Modem Status Response
rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1
@ -295,12 +295,12 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
} else {
if(rfcommChannelType == RFCOMM_SABM) { // SABM Command - this is sent twice: once for channel 0 and then for the channel to establish
#ifdef DEBUG
Notify(PSTR("\r\nReceived SABM Command"));
#endif
Notify(PSTR("\r\nReceived SABM Command"), 0x80);
#endif
sendRfcomm(rfcommChannel,rfcommDirection,rfcommCommandResponse,RFCOMM_UA,rfcommPfBit,rfcommbuf,0x00); // UA Command
} else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_PN_CMD) { // UIH Parameter Negotiation Command
#ifdef DEBUG
Notify(PSTR("\r\nReceived UIH Parameter Negotiation Command"));
Notify(PSTR("\r\nReceived UIH Parameter Negotiation Command"), 0x80);
#endif
rfcommbuf[0] = BT_RFCOMM_PN_RSP; // UIH Parameter Negotiation Response
rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1
@ -315,28 +315,28 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x0A);
} else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command
#ifdef DEBUG
Notify(PSTR("\r\nSend UIH Modem Status Response"));
Notify(PSTR("\r\nSend UIH Modem Status Response"), 0x80);
#endif
rfcommbuf[0] = BT_RFCOMM_MSC_RSP; // UIH Modem Status Response
rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1
rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3)
rfcommbuf[3] = l2capinbuf[14];
sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x04);
delay(1);
#ifdef DEBUG
Notify(PSTR("\r\nSend UIH Modem Status Command"));
Notify(PSTR("\r\nSend UIH Modem Status Command"), 0x80);
#endif
rfcommbuf[0] = BT_RFCOMM_MSC_CMD; // UIH Modem Status Command
rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1
rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3)
rfcommbuf[3] = 0x8D; // Can receive frames (YES), Ready to Communicate (YES), Ready to Receive (YES), Incomig Call (NO), Data is Value (YES)
sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x04);
} else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_RSP) { // UIH Modem Status Response
if(!creditSent) {
#ifdef DEBUG
Notify(PSTR("\r\nSend UIH Command with credit"));
Notify(PSTR("\r\nSend UIH Command with credit"), 0x80);
#endif
sendRfcommCredit(rfcommChannelConnection,rfcommDirection,0,RFCOMM_UIH,0x10,sizeof(rfcommDataBuffer)); // Send credit
creditSent = true;
@ -345,11 +345,11 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
}
} else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[10] == 0x01) { // UIH Command with credit
#ifdef DEBUG
Notify(PSTR("\r\nReceived UIH Command with credit"));
#endif
Notify(PSTR("\r\nReceived UIH Command with credit"), 0x80);
#endif
} else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command
#ifdef DEBUG
Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command"));
Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command"), 0x80);
#endif
rfcommbuf[0] = BT_RFCOMM_RPN_RSP; // Command
rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1
@ -363,28 +363,28 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
rfcommbuf[9] = l2capinbuf[20]; // Number of Frames
sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x0A); // UIH Remote Port Negotiation Response
#ifdef DEBUG
Notify(PSTR("\r\nRFCOMM Connection is now established\r\n"));
#endif
Notify(PSTR("\r\nRFCOMM Connection is now established\r\n"), 0x80);
#endif
waitForLastCommand = false;
creditSent = false;
connected = true; // The RFCOMM channel is now established
}
#ifdef DEBUG
else if(rfcommChannelType != RFCOMM_DISC) {
Notify(PSTR("\r\nUnsupported RFCOMM Data - ChannelType: "));
PrintHex<uint8_t>(rfcommChannelType);
Notify(PSTR(" Command: "));
PrintHex<uint8_t>(l2capinbuf[11]);
Notify(PSTR("\r\nUnsupported RFCOMM Data - ChannelType: "), 0x80);
PrintHex<uint8_t>(rfcommChannelType, 0x80);
Notify(PSTR(" Command: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[11], 0x80);
}
#endif
#endif
}
}
#ifdef EXTRADEBUG
else {
Notify(PSTR("\r\nUnsupported L2CAP Data - Channel ID: "));
PrintHex<uint8_t>(l2capinbuf[7]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[6]);
Notify(PSTR("\r\nUnsupported L2CAP Data - Channel ID: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[7], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[6], 0x80);
}
#endif
SDP_task();
@ -394,12 +394,12 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
void SPP::Run() {
if(waitForLastCommand && (millis() - timer) > 100) { // We will only wait 100ms and see if the UIH Remote Port Negotiation Command is send, as some deviced don't send it
#ifdef DEBUG
Notify(PSTR("\r\nRFCOMM Connection is now established - Automatic\r\n"));
Notify(PSTR("\r\nRFCOMM Connection is now established - Automatic\r\n"), 0x80);
#endif
creditSent = false;
waitForLastCommand = false;
connected = true; // The RFCOMM channel is now established
}
}
}
void SPP::SDP_task() {
switch (l2cap_sdp_state)
@ -408,24 +408,24 @@ void SPP::SDP_task() {
if (l2cap_connection_request_sdp_flag) {
l2cap_event_flag &= ~L2CAP_FLAG_CONNECTION_SDP_REQUEST; // Clear flag
#ifdef DEBUG
Notify(PSTR("\r\nSDP Incoming Connection Request"));
Notify(PSTR("\r\nSDP Incoming Connection Request"), 0x80);
#endif
pBtd->l2cap_connection_response(hci_handle,identifier, sdp_dcid, sdp_scid, PENDING);
delay(1);
pBtd->l2cap_connection_response(hci_handle,identifier, sdp_dcid, sdp_scid, SUCCESSFUL);
pBtd->l2cap_connection_response(hci_handle,identifier, sdp_dcid, sdp_scid, SUCCESSFUL);
identifier++;
delay(1);
pBtd->l2cap_config_request(hci_handle,identifier, sdp_scid);
l2cap_sdp_state = L2CAP_SDP_REQUEST;
}
pBtd->l2cap_config_request(hci_handle,identifier, sdp_scid);
l2cap_sdp_state = L2CAP_SDP_REQUEST;
}
break;
case L2CAP_SDP_REQUEST:
if (l2cap_config_request_sdp_flag) {
l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_SDP_REQUEST; // Clear flag
#ifdef DEBUG
Notify(PSTR("\r\nSDP Configuration Request"));
#endif
pBtd->l2cap_config_response(hci_handle,identifier, sdp_scid);
Notify(PSTR("\r\nSDP Configuration Request"), 0x80);
#endif
pBtd->l2cap_config_response(hci_handle,identifier, sdp_scid);
l2cap_sdp_state = L2CAP_SDP_SUCCESS;
}
break;
@ -433,7 +433,7 @@ void SPP::SDP_task() {
if (l2cap_config_success_sdp_flag) {
l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_SDP_SUCCESS; // Clear flag
#ifdef DEBUG
Notify(PSTR("\r\nSDP Successfully Configured"));
Notify(PSTR("\r\nSDP Successfully Configured"), 0x80);
#endif
firstMessage = true; // Reset bool
SDPConnected = true;
@ -445,9 +445,9 @@ void SPP::SDP_task() {
l2cap_event_flag &= ~L2CAP_FLAG_DISCONNECT_SDP_REQUEST; // Clear flag
SDPConnected = false;
#ifdef DEBUG
Notify(PSTR("\r\nDisconnected SDP Channel"));
Notify(PSTR("\r\nDisconnected SDP Channel"), 0x80);
#endif
pBtd->l2cap_disconnection_response(hci_handle,identifier,sdp_dcid,sdp_scid);
pBtd->l2cap_disconnection_response(hci_handle,identifier,sdp_dcid,sdp_scid);
l2cap_sdp_state = L2CAP_SDP_WAIT;
} else if(l2cap_connection_request_sdp_flag)
l2cap_rfcomm_state = L2CAP_SDP_WAIT;
@ -455,7 +455,7 @@ void SPP::SDP_task() {
case L2CAP_DISCONNECT_RESPONSE: // This is for both disconnection response from the RFCOMM and SDP channel if they were connected
if (l2cap_disconnect_response_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nDisconnected L2CAP Connection"));
Notify(PSTR("\r\nDisconnected L2CAP Connection"), 0x80);
#endif
RFCOMMConnected = false;
SDPConnected = false;
@ -465,35 +465,35 @@ void SPP::SDP_task() {
l2cap_sdp_state = L2CAP_SDP_WAIT;
l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT;
}
break;
break;
}
}
void SPP::RFCOMM_task()
{
{
switch (l2cap_rfcomm_state)
{
case L2CAP_RFCOMM_WAIT:
case L2CAP_RFCOMM_WAIT:
if(l2cap_connection_request_rfcomm_flag) {
l2cap_event_flag &= ~L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST; // Clear flag
#ifdef DEBUG
Notify(PSTR("\r\nRFCOMM Incoming Connection Request"));
Notify(PSTR("\r\nRFCOMM Incoming Connection Request"), 0x80);
#endif
pBtd->l2cap_connection_response(hci_handle,identifier, rfcomm_dcid, rfcomm_scid, PENDING);
pBtd->l2cap_connection_response(hci_handle,identifier, rfcomm_dcid, rfcomm_scid, PENDING);
delay(1);
pBtd->l2cap_connection_response(hci_handle,identifier, rfcomm_dcid, rfcomm_scid, SUCCESSFUL);
pBtd->l2cap_connection_response(hci_handle,identifier, rfcomm_dcid, rfcomm_scid, SUCCESSFUL);
identifier++;
delay(1);
pBtd->l2cap_config_request(hci_handle,identifier, rfcomm_scid);
l2cap_rfcomm_state = L2CAP_RFCOMM_REQUEST;
}
break;
pBtd->l2cap_config_request(hci_handle,identifier, rfcomm_scid);
l2cap_rfcomm_state = L2CAP_RFCOMM_REQUEST;
}
break;
case L2CAP_RFCOMM_REQUEST:
if (l2cap_config_request_rfcomm_flag) {
l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_RFCOMM_REQUEST; // Clear flag
#ifdef DEBUG
Notify(PSTR("\r\nRFCOMM Configuration Request"));
Notify(PSTR("\r\nRFCOMM Configuration Request"), 0x80);
#endif
pBtd->l2cap_config_response(hci_handle,identifier, rfcomm_scid);
pBtd->l2cap_config_response(hci_handle,identifier, rfcomm_scid);
l2cap_rfcomm_state = L2CAP_RFCOMM_SUCCESS;
}
break;
@ -501,27 +501,27 @@ void SPP::RFCOMM_task()
if (l2cap_config_success_rfcomm_flag) {
l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS; // Clear flag
#ifdef DEBUG
Notify(PSTR("\r\nRFCOMM Successfully Configured"));
#endif
Notify(PSTR("\r\nRFCOMM Successfully Configured"), 0x80);
#endif
rfcommAvailable = 0; // Reset number of bytes available
bytesRead = 0; // Reset number of bytes received
RFCOMMConnected = true;
l2cap_rfcomm_state = L2CAP_RFCOMM_DONE;
}
break;
break;
case L2CAP_RFCOMM_DONE:
if(l2cap_disconnect_request_rfcomm_flag) {
l2cap_event_flag &= ~L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST; // Clear flag
RFCOMMConnected = false;
connected = false;
#ifdef DEBUG
Notify(PSTR("\r\nDisconnected RFCOMM Channel"));
Notify(PSTR("\r\nDisconnected RFCOMM Channel"), 0x80);
#endif
pBtd->l2cap_disconnection_response(hci_handle,identifier,rfcomm_dcid,rfcomm_scid);
l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT;
} else if(l2cap_connection_request_rfcomm_flag)
l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT;
break;
break;
}
}
/************************************************************/
@ -538,12 +538,12 @@ void SPP::serviceNotSupported(uint8_t transactionIDHigh, uint8_t transactionIDLo
l2capoutbuf[4] = 0x05; // Parameter Length
l2capoutbuf[5] = 0x00; // AttributeListsByteCount
l2capoutbuf[6] = 0x02; // AttributeListsByteCount
/* Attribute ID/Value Sequence: */
l2capoutbuf[7] = 0x35;
l2capoutbuf[8] = 0x00;
l2capoutbuf[9] = 0x00;
SDP_Command(l2capoutbuf,10);
}
void SPP::serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow) {
@ -561,7 +561,7 @@ void SPP::serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLo
l2capoutbuf[9] = 0x3C;
l2capoutbuf[10] = 0x36;
l2capoutbuf[11] = 0x00;
l2capoutbuf[12] = 0x39;
l2capoutbuf[13] = 0x09;
l2capoutbuf[14] = 0x00;
@ -578,7 +578,7 @@ void SPP::serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLo
l2capoutbuf[25] = 0x03;
l2capoutbuf[26] = 0x19;
l2capoutbuf[27] = 0x11;
l2capoutbuf[28] = 0x01;
l2capoutbuf[29] = 0x09;
l2capoutbuf[30] = 0x00;
@ -595,13 +595,13 @@ void SPP::serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLo
l2capoutbuf[41] = 0x19;
l2capoutbuf[42] = 0x00;
l2capoutbuf[43] = 0x03;
l2capoutbuf[44] = 0x08;
l2capoutbuf[45] = 0x02; // Two extra bytes
l2capoutbuf[46] = 0x00; // 25 (0x19) more bytes to come
l2capoutbuf[47] = 0x19;
SDP_Command(l2capoutbuf,48);
l2capoutbuf[47] = 0x19;
SDP_Command(l2capoutbuf,48);
}
void SPP::serialPortResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow) {
l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU;
@ -611,14 +611,14 @@ void SPP::serialPortResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLo
l2capoutbuf[4] = 0x1C; // Parameter Length
l2capoutbuf[5] = 0x00; // AttributeListsByteCount
l2capoutbuf[6] = 0x19; // AttributeListsByteCount
/* Attribute ID/Value Sequence: */
l2capoutbuf[7] = 0x01;
l2capoutbuf[8] = 0x09;
l2capoutbuf[9] = 0x00;
l2capoutbuf[10] = 0x06;
l2capoutbuf[11] = 0x35;
l2capoutbuf[12] = 0x09;
l2capoutbuf[13] = 0x09;
l2capoutbuf[14] = 0x65;
@ -634,7 +634,7 @@ void SPP::serialPortResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLo
l2capoutbuf[24] = 0x00;
l2capoutbuf[25] = 0x25;
l2capoutbuf[26] = 0x05; // Name length
l2capoutbuf[26] = 0x05; // Name length
l2capoutbuf[27] = 'T';
l2capoutbuf[28] = 'K';
l2capoutbuf[29] = 'J';
@ -648,7 +648,7 @@ void SPP::l2capResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow) {
serialPortResponse1(transactionIDHigh,transactionIDLow); // These has to send all the supported functions, since it only supports virtual serialport it just sends the message again
}
void SPP::l2capResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow) {
serialPortResponse2(transactionIDHigh,transactionIDLow); // Same data as serialPortResponse2
serialPortResponse2(transactionIDHigh,transactionIDLow); // Same data as serialPortResponse2
}
/************************************************************/
/* RFCOMM Commands */
@ -666,26 +666,26 @@ void SPP::sendRfcomm(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t cha
l2capoutbuf[i+3] = data[i];
l2capoutbuf[i+3] = calcFcs(l2capoutbuf);
#ifdef EXTRADEBUG
Notify(PSTR(" - RFCOMM Data: "));
Notify(PSTR(" - RFCOMM Data: "), 0x80);
for(i = 0; i < length+4; i++) {
Serial.print(l2capoutbuf[i],HEX);
Notify(PSTR(" "));
Notify(PSTR(" "), 0x80);
}
#endif
#endif
RFCOMM_Command(l2capoutbuf,length+4);
}
}
void SPP::sendRfcommCredit(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t credit) {
l2capoutbuf[0] = channel | direction | CR | extendAddress; // RFCOMM Address
l2capoutbuf[1] = channelType | pfBit; // RFCOMM Control
l2capoutbuf[2] = 0x01; // Length = 0
l2capoutbuf[3] = credit; // Credit
l2capoutbuf[4] = calcFcs(l2capoutbuf);
l2capoutbuf[4] = calcFcs(l2capoutbuf);
#ifdef EXTRADEBUG
Notify(PSTR(" - RFCOMM Credit Data: "));
Notify(PSTR(" - RFCOMM Credit Data: "), 0x80);
for(uint8_t i = 0; i < 5; i++) {
Serial.print(l2capoutbuf[i],HEX);
Notify(PSTR(" "));
Notify(PSTR(" "), 0x80);
}
#endif
RFCOMM_Command(l2capoutbuf,5);
@ -714,11 +714,11 @@ void SPP::print(const String &str) {
l2capoutbuf[0] = rfcommChannelConnection | 0 | 0 | extendAddress;; // RFCOMM Address
l2capoutbuf[1] = RFCOMM_UIH; // RFCOMM Control
l2capoutbuf[2] = length << 1 | 1; // Length
uint8_t i = 0;
uint8_t i = 0;
for(; i < length; i++)
l2capoutbuf[i+3] = str[i];
l2capoutbuf[i+3] = calcFcs(l2capoutbuf);
RFCOMM_Command(l2capoutbuf,length+4);
}
void SPP::print(const char* str) {
@ -728,13 +728,13 @@ void SPP::print(const char* str) {
if(length > (sizeof(l2capoutbuf)-4))
length = sizeof(l2capoutbuf)-4;
l2capoutbuf[0] = rfcommChannelConnection | 0 | 0 | extendAddress;; // RFCOMM Address
l2capoutbuf[1] = RFCOMM_UIH; // RFCOMM Control
l2capoutbuf[1] = RFCOMM_UIH; // RFCOMM Control
l2capoutbuf[2] = length << 1 | 1; // Length
uint8_t i = 0;
for(; i < length; i++)
l2capoutbuf[i+3] = str[i];
l2capoutbuf[i+3] = calcFcs(l2capoutbuf);
l2capoutbuf[i+3] = str[i];
l2capoutbuf[i+3] = calcFcs(l2capoutbuf);
RFCOMM_Command(l2capoutbuf,length+4);
}
void SPP::print(uint8_t* array, uint8_t length) {
@ -749,7 +749,7 @@ void SPP::print(uint8_t* array, uint8_t length) {
for(; i < length; i++)
l2capoutbuf[i+3] = array[i];
l2capoutbuf[i+3] = calcFcs(l2capoutbuf);
RFCOMM_Command(l2capoutbuf,length+4);
}
void SPP::println(const String &str) {
@ -783,10 +783,10 @@ void SPP::printFlashString(const __FlashStringHelper *ifsh, bool newline) {
size++;
}
uint8_t buf[size+2]; // Add two extra in case it needs to print a newline and carriage return
for(uint8_t i = 0; i < size; i++)
buf[i] = pgm_read_byte(p++);
if(newline) {
buf[size] = '\r';
buf[size+1] = '\n';
@ -868,13 +868,13 @@ void SPP::doubleToString(double input, char* output, uint8_t digits) {
}
else
strcpy(output,"");
// Round correctly
double rounding = 0.5;
for (uint8_t i=0; i<digits; i++)
rounding /= 10.0;
input += rounding;
uint32_t intpart = (uint32_t)input;
intToString(intpart,buffer); // Convert to string
strcat(output,buffer);
@ -902,9 +902,9 @@ uint8_t SPP::read() {
bytesRead = 0;
sendRfcommCredit(rfcommChannelConnection,rfcommDirection,0,RFCOMM_UIH,0x10,sizeof(rfcommDataBuffer)); // Send more credit
#ifdef EXTRADEBUG
Notify(PSTR("\r\nSent "));
Notify(PSTR("\r\nSent "), 0x80);
Serial.print(sizeof(rfcommDataBuffer));
Notify(PSTR(" more credit"));
Notify(PSTR(" more credit"), 0x80);
#endif
}
return output;

326
Wii.cpp
View file

@ -1,15 +1,15 @@
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
@ -29,7 +29,7 @@ const uint8_t LEDS[] PROGMEM = {
0x20, // LED2
0x40, // LED3
0x80, // LED4
0x90, // LED5
0xA0, // LED6
0xC0, // LED7
@ -88,17 +88,17 @@ pBtd(p) // pointer to USB class instance - mandatory
{
if (pBtd)
pBtd->registerServiceClass(this); // Register it as a Bluetooth service
pBtd->pairWithWii = pair;
HIDBuffer[0] = 0xA2;// HID BT DATA_request (0xA0) | Report Type (Output 0x02)
/* Set device cid for the control and intterrupt channelse - LSB */
control_dcid[0] = 0x60;//0x0060
control_dcid[1] = 0x00;
interrupt_dcid[0] = 0x61;//0x0061
interrupt_dcid[1] = 0x00;
Reset();
}
void WII::Reset() {
@ -117,10 +117,10 @@ void WII::Reset() {
void WII::disconnect() { // Use this void to disconnect any of the controllers
if(motionPlusConnected && !pBtd->motionPlusInside) { // Disable the Motion Plus extension
#ifdef DEBUG
Notify(PSTR("\r\nDeactivating Motion Plus"));
#ifdef DEBUG
Notify(PSTR("\r\nDeactivating Motion Plus"), 0x80);
#endif
initExtension1(); // This will disable the Motion Plus extension
initExtension1(); // This will disable the Motion Plus extension
}
//First the HID interrupt channel has to be disconencted, then the HID control channel and finally the HCI connection
pBtd->l2cap_disconnection_request(hci_handle,0x0A, interrupt_scid, interrupt_dcid);
@ -144,18 +144,18 @@ void WII::ACLData(uint8_t* l2capinbuf) {
if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U
if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
#ifdef DEBUG
Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "));
PrintHex<uint8_t>(l2capinbuf[13]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[12]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[17]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[16]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[15]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[14]);
Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[13], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[12], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[17], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[16], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[15], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[14], 0x80);
#endif
}
else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_RESPONSE) {
@ -175,19 +175,19 @@ void WII::ACLData(uint8_t* l2capinbuf) {
l2cap_event_flag |= L2CAP_FLAG_INTERRUPT_CONNECTED;
}
}
}
}
else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
#ifdef EXTRADEBUG
Notify(PSTR("\r\nL2CAP Connection Request - PSM: "));
PrintHex<uint8_t>(l2capinbuf[13]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[12]);
Notify(PSTR(" SCID: "));
PrintHex<uint8_t>(l2capinbuf[15]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[14]);
Notify(PSTR(" Identifier: "));
PrintHex<uint8_t>(l2capinbuf[9]);
Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[13], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[12], 0x80);
Notify(PSTR(" SCID: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[15], 0x80);
Notify(PSTR(" "), 0x80);
PrintHex<uint8_t>(l2capinbuf[14], 0x80);
Notify(PSTR(" Identifier: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[9], 0x80);
#endif
if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
identifier = l2capinbuf[9];
@ -211,7 +211,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
}
else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
//Serial.print("\r\nHID Interrupt Configuration Complete");
identifier = l2capinbuf[9];
identifier = l2capinbuf[9];
l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS;
}
}
@ -229,18 +229,18 @@ void WII::ACLData(uint8_t* l2capinbuf) {
else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
#ifdef DEBUG
Notify(PSTR("\r\nDisconnect Request: Control Channel"));
Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80);
#endif
identifier = l2capinbuf[9];
pBtd->l2cap_disconnection_response(hci_handle,identifier,control_dcid,control_scid);
pBtd->l2cap_disconnection_response(hci_handle,identifier,control_dcid,control_scid);
Reset();
}
else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
#ifdef DEBUG
Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"));
Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80);
#endif
identifier = l2capinbuf[9];
pBtd->l2cap_disconnection_response(hci_handle,identifier,interrupt_dcid,interrupt_scid);
pBtd->l2cap_disconnection_response(hci_handle,identifier,interrupt_dcid,interrupt_scid);
Reset();
}
}
@ -259,8 +259,8 @@ void WII::ACLData(uint8_t* l2capinbuf) {
#ifdef EXTRADEBUG
else {
identifier = l2capinbuf[9];
Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "));
PrintHex<uint8_t>(l2capinbuf[8]);
Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[8], 0x80);
}
#endif
} else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
@ -285,11 +285,11 @@ void WII::ACLData(uint8_t* l2capinbuf) {
else if(!unknownExtensionConnected)
ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
#ifdef PRINTREPORT
Notify(PSTR("ButtonState: "));
PrintHex<uint32_t>(ButtonState);
Notify(PSTR("\r\n"));
Notify(PSTR("ButtonState: "), 0x80);
PrintHex<uint32_t>(ButtonState, 0x80);
Notify(PSTR("\r\n"), 0x80);
#endif
if(ButtonState != OldButtonState) {
if(ButtonState != OldButtonState) {
ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable
OldButtonState = ButtonState;
}
@ -297,7 +297,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
if(l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33 || l2capinbuf[9] == 0x35 || l2capinbuf[9] == 0x37) { // Read the accelerometer
accX = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5))-500;
accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4))-500;
accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5))-500;
accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5))-500;
wiimotePitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
wiimoteRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
}
@ -307,27 +307,27 @@ void WII::ACLData(uint8_t* l2capinbuf) {
batteryLevel = l2capinbuf[15]; // Update battery level
if(l2capinbuf[12] & 0x01) {
#ifdef DEBUG
Notify(PSTR("\r\nWARNING: Battery is nearly empty"));
#endif
Notify(PSTR("\r\nWARNING: Battery is nearly empty"), 0x80);
#endif
}
if(l2capinbuf[12] & 0x02) { // Check if a extension is connected
#ifdef DEBUG
if(!unknownExtensionConnected)
Notify(PSTR("\r\nExtension connected"));
Notify(PSTR("\r\nExtension connected"), 0x80);
#endif
unknownExtensionConnected = true;
#ifdef WIICAMERA
if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
#endif
#endif
setReportMode(false,0x35); // Also read the extension
}
else {
#ifdef DEBUG
Notify(PSTR("\r\nExtension disconnected"));
Notify(PSTR("\r\nExtension disconnected"), 0x80);
#endif
if(motionPlusConnected) {
#ifdef DEBUG
Notify(PSTR(" - from Motion Plus"));
Notify(PSTR(" - from Motion Plus"), 0x80);
#endif
l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED;
if(!activateNunchuck) // If it's already trying to initialize the Nunchuck don't set it to false
@ -336,82 +336,82 @@ void WII::ACLData(uint8_t* l2capinbuf) {
}
else if(nunchuckConnected) {
#ifdef DEBUG
Notify(PSTR(" - Nunchuck"));
Notify(PSTR(" - Nunchuck"), 0x80);
#endif
nunchuckConnected = false; // It must be the Nunchuck controller then
l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED;
setLedStatus();
setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer
} else {
setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer
setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer
}
}
break;
case 0x21: // Read Memory Data
if((l2capinbuf[12] & 0x0F) == 0) { // No error
// See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers
if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) {
if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) {
#ifdef DEBUG
Notify(PSTR("\r\nNunchuck connected"));
Notify(PSTR("\r\nNunchuck connected"), 0x80);
#endif
l2cap_event_flag |= WII_FLAG_NUNCHUCK_CONNECTED;
l2cap_event_flag |= WII_FLAG_NUNCHUCK_CONNECTED;
} else if(l2capinbuf[16] == 0x00 && (l2capinbuf[17] == 0xA6 || l2capinbuf[17] == 0xA4) && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) {
#ifdef DEBUG
Notify(PSTR("\r\nMotion Plus connected"));
Notify(PSTR("\r\nMotion Plus connected"), 0x80);
#endif
l2cap_event_flag |= WII_FLAG_MOTION_PLUS_CONNECTED;
} else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) {
#ifdef DEBUG
Notify(PSTR("\r\nMotion Plus activated in normal mode"));
Notify(PSTR("\r\nMotion Plus activated in normal mode"), 0x80);
#endif
motionPlusConnected = true;
} else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) {
#ifdef DEBUG
Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode"));
Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode"), 0x80);
#endif
activateNunchuck = false;
motionPlusConnected = true;
nunchuckConnected = true;
} else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA6 && l2capinbuf[18] == 0x20 && (l2capinbuf[19] == 0x00 || l2capinbuf[19] == 0x04 || l2capinbuf[19] == 0x05 || l2capinbuf[19] == 0x07) && l2capinbuf[20] == 0x05) {
#ifdef DEBUG
Notify(PSTR("\r\nInactive Wii Motion Plus"));
Notify(PSTR("\r\nPlease unplug the Motion Plus, disconnect the Wiimote and then replug the Motion Plus Extension"));
Notify(PSTR("\r\nInactive Wii Motion Plus"), 0x80);
Notify(PSTR("\r\nPlease unplug the Motion Plus, disconnect the Wiimote and then replug the Motion Plus Extension"), 0x80);
#endif
stateCounter = 300; // Skip the rest in "L2CAP_CHECK_MOTION_PLUS_STATE"
} else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x01 && l2capinbuf[20] == 0x20) {
#ifdef DEBUG
Notify(PSTR("\r\nWii U Pro Controller connected"));
Notify(PSTR("\r\nWii U Pro Controller connected"), 0x80);
#endif
wiiUProControllerConnected = true;
}
#ifdef DEBUG
else {
Notify(PSTR("\r\nUnknown Device: "));
PrintHex<uint8_t>(l2capinbuf[13]);
PrintHex<uint8_t>(l2capinbuf[14]);
Notify(PSTR("\r\nData: "));
Notify(PSTR("\r\nUnknown Device: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[13], 0x80);
PrintHex<uint8_t>(l2capinbuf[14], 0x80);
Notify(PSTR("\r\nData: "), 0x80);
for(uint8_t i = 0; i < ((l2capinbuf[12] >> 4)+1); i++) { // bit 4-7 is the length-1
PrintHex<uint8_t>(l2capinbuf[15+i]);
Notify(PSTR(" "));
PrintHex<uint8_t>(l2capinbuf[15+i], 0x80);
Notify(PSTR(" "), 0x80);
}
}
#endif
}
#ifdef EXTRADEBUG
else {
Notify(PSTR("\r\nReport Error: "));
PrintHex<uint8_t>(l2capinbuf[13]);
PrintHex<uint8_t>(l2capinbuf[14]);
Notify(PSTR("\r\nReport Error: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[13], 0x80);
PrintHex<uint8_t>(l2capinbuf[14], 0x80);
}
#endif
break;
case 0x22: // Acknowledge output report, return function result
#ifdef DEBUG
if(l2capinbuf[13] != 0x00) { // Check if there is an error
Notify(PSTR("\r\nCommand failed: "));
PrintHex<uint8_t>(l2capinbuf[12]);
Notify(PSTR("\r\nCommand failed: "), 0x80);
PrintHex<uint8_t>(l2capinbuf[12], 0x80);
}
#endif
#endif
break;
case 0x30: // Core buttons - (a1) 30 BB BB
break;
@ -425,7 +425,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus data available
roll = wiimoteRoll;
#ifdef WIICAMERA
// Read the IR data
// Read the IR data
IR_object_x1 = (l2capinbuf[15] | ((uint16_t)(l2capinbuf[17] & 0x30) << 4)); // x position
IR_object_y1 = (l2capinbuf[16] | ((uint16_t)(l2capinbuf[17] & 0xC0) << 2)); // y position
IR_object_s1 = (l2capinbuf[17] & 0x0F); // size value, 0-15
@ -447,7 +447,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
break;
/* 0x3e and 0x3f both give unknown report types when report mode is 0x3e or 0x3f with mode number 0x05 */
case 0x3E: // Core Buttons with Accelerometer and 32 IR bytes
// (a1) 31 BB BB AA AA AA II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II
// (a1) 31 BB BB AA AA AA II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II
// corresponds to output report mode 0x3e
/**** for reading in full mode: DOES NOT WORK YET ****/
@ -456,7 +456,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4));
IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2));
IR_object_s1 = (l2capinbuf[15] & 0x0F);
*/
*/
break;
case 0x3F:
/*
@ -466,7 +466,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
*/
break;
case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes
// (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
// (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
if(motionPlusConnected) {
if(l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension
if(motionValuesReset) { // We will only use the values when the gyro value has been set
@ -477,7 +477,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
yawGyroSpeed = (double)gyroYawRaw/((double)gyroYawZero/yawGyroScale);
rollGyroSpeed = -(double)gyroRollRaw/((double)gyroRollZero/rollGyroScale); // We invert these values so they will fit the acc values
pitchGyroSpeed = (double)gyroPitchRaw/((double)gyroPitchZero/pitchGyroScale);
/* The onboard gyro has two ranges for slow and fast mode */
if(!(l2capinbuf[18] & 0x02)) // Check if fast more is used
yawGyroSpeed *= 4.545;
@ -485,10 +485,10 @@ void WII::ACLData(uint8_t* l2capinbuf) {
pitchGyroSpeed *= 4.545;
if(!(l2capinbuf[19] & 0x02)) // Check if fast more is used
rollGyroSpeed *= 4.545;
pitch = (0.93*(pitch+(pitchGyroSpeed*(double)(micros()-timer)/1000000)))+(0.07*wiimotePitch); // Use a complimentary filter to calculate the angle
roll = (0.93*(roll+(rollGyroSpeed*(double)(micros()-timer)/1000000)))+(0.07*wiimoteRoll);
gyroYaw += (yawGyroSpeed*((double)(micros()-timer)/1000000));
gyroRoll += (rollGyroSpeed*((double)(micros()-timer)/1000000));
gyroPitch += (pitchGyroSpeed*((double)(micros()-timer)/1000000));
@ -503,20 +503,20 @@ void WII::ACLData(uint8_t* l2capinbuf) {
Serial.print(gyroPitch);
*/
/*
Serial.print("\twiimoteRoll: ");
Serial.print("\twiimoteRoll: ");
Serial.print(wiimoteRoll);
Serial.print("\twiimotePitch: ");
Serial.print(wiimotePitch);
*/
*/
} else {
if((micros() - timer) > 1000000) { // Loop for 1 sec before resetting the values
#ifdef DEBUG
Notify(PSTR("\r\nThe gyro values has been reset"));
Notify(PSTR("\r\nThe gyro values has been reset"), 0x80);
#endif
gyroYawZero = (l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6));
gyroRollZero = (l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6));
gyroPitchZero = (l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6));
rollGyroScale = 500; // You might need to adjust these
pitchGyroScale = 400;
yawGyroScale = 415;
@ -524,10 +524,10 @@ void WII::ACLData(uint8_t* l2capinbuf) {
gyroYaw = 0;
gyroRoll = 0;
gyroPitch = 0;
motionValuesReset = true;
timer = micros();
}
}
}
} else {
if(nunchuckConnected) {
@ -539,14 +539,14 @@ void WII::ACLData(uint8_t* l2capinbuf) {
nunchuckPitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
nunchuckRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
}
//else if(classicControllerConnected) { }
//else if(classicControllerConnected) { }
}
if(l2capinbuf[19] & 0x01) {
if(!extensionConnected) {
extensionConnected = true;
unknownExtensionConnected = true;
#ifdef DEBUG
Notify(PSTR("\r\nExtension connected to Motion Plus"));
Notify(PSTR("\r\nExtension connected to Motion Plus"), 0x80);
#endif
}
}
@ -555,12 +555,12 @@ void WII::ACLData(uint8_t* l2capinbuf) {
extensionConnected = false;
unknownExtensionConnected = true;
#ifdef DEBUG
Notify(PSTR("\r\nExtension disconnected from Motion Plus"));
Notify(PSTR("\r\nExtension disconnected from Motion Plus"), 0x80);
#endif
nunchuckConnected = false; // There is no extension connected to the Motion Plus if this report is sent
}
}
} else if(nunchuckConnected) {
hatValues[HatX] = l2capinbuf[15];
hatValues[HatY] = l2capinbuf[16];
@ -569,7 +569,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
accZ = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6))-416;
nunchuckPitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
nunchuckRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected
roll = wiimoteRoll;
} else if(wiiUProControllerConnected) {
@ -581,11 +581,11 @@ void WII::ACLData(uint8_t* l2capinbuf) {
break;
#ifdef DEBUG
default:
Notify(PSTR("\r\nUnknown Report type: "));
Notify(PSTR("\r\nUnknown Report type: "), 0x80);
Serial.print(l2capinbuf[9],HEX);
break;
#endif
}
}
}
}
}
@ -598,16 +598,16 @@ void WII::L2CAP_task() {
case L2CAP_CONTROL_SUCCESS:
if (l2cap_config_success_control_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nHID Control Successfully Configured"));
Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80);
#endif
l2cap_state = L2CAP_INTERRUPT_SETUP;
}
break;
case L2CAP_INTERRUPT_SETUP:
if (l2cap_connection_request_interrupt_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"));
Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80);
#endif
pBtd->l2cap_connection_response(hci_handle,identifier, interrupt_dcid, interrupt_scid, PENDING);
delay(1);
@ -615,7 +615,7 @@ void WII::L2CAP_task() {
identifier++;
delay(1);
pBtd->l2cap_config_request(hci_handle,identifier, interrupt_scid);
l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST;
}
break;
@ -624,40 +624,40 @@ void WII::L2CAP_task() {
case L2CAP_CONTROL_CONNECT_REQUEST:
if (l2cap_connected_control_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nSend HID Control Config Request"));
Notify(PSTR("\r\nSend HID Control Config Request"), 0x80);
#endif
identifier++;
pBtd->l2cap_config_request(hci_handle, identifier, control_scid);
l2cap_state = L2CAP_CONTROL_CONFIG_REQUEST;
}
break;
case L2CAP_CONTROL_CONFIG_REQUEST:
if(l2cap_config_success_control_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nSend HID Interrupt Connection Request"));
Notify(PSTR("\r\nSend HID Interrupt Connection Request"), 0x80);
#endif
identifier++;
pBtd->l2cap_connection_request(hci_handle,identifier,interrupt_dcid,HID_INTR_PSM);
l2cap_state = L2CAP_INTERRUPT_CONNECT_REQUEST;
}
break;
case L2CAP_INTERRUPT_CONNECT_REQUEST:
if(l2cap_connected_interrupt_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nSend HID Interrupt Config Request"));
Notify(PSTR("\r\nSend HID Interrupt Config Request"), 0x80);
#endif
identifier++;
pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid);
l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST;
}
break;
case L2CAP_INTERRUPT_CONFIG_REQUEST:
if(l2cap_config_success_interrupt_flag) { // Now the HID channels is established
#ifdef DEBUG
Notify(PSTR("\r\nHID Channels Established"));
Notify(PSTR("\r\nHID Channels Established"), 0x80);
#endif
pBtd->connectToWii = false;
pBtd->pairWithWii = false;
@ -666,24 +666,24 @@ void WII::L2CAP_task() {
l2cap_state = L2CAP_CHECK_MOTION_PLUS_STATE;
}
break;
/* The next states are in run() */
/* The next states are in run() */
case L2CAP_INTERRUPT_DISCONNECT:
if (l2cap_disconnect_response_interrupt_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nDisconnected Interrupt Channel"));
Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80);
#endif
identifier++;
pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid);
pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid);
l2cap_state = L2CAP_CONTROL_DISCONNECT;
}
break;
case L2CAP_CONTROL_DISCONNECT:
if (l2cap_disconnect_response_control_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nDisconnected Control Channel"));
Notify(PSTR("\r\nDisconnected Control Channel"), 0x80);
#endif
pBtd->hci_disconnect(hci_handle);
hci_handle = -1; // Reset handle
@ -691,7 +691,7 @@ void WII::L2CAP_task() {
l2cap_state = L2CAP_WAIT;
}
break;
}
}
}
void WII::Run() {
switch (l2cap_state) {
@ -700,16 +700,16 @@ void WII::Run() {
pBtd->l2capConnectionClaimed = true;
activeConnection = true;
#ifdef DEBUG
Notify(PSTR("\r\nSend HID Control Connection Request"));
Notify(PSTR("\r\nSend HID Control Connection Request"), 0x80);
#endif
hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
l2cap_event_flag = 0; // Reset flags
identifier = 0;
pBtd->l2cap_connection_request(hci_handle,identifier,control_dcid,HID_CTRL_PSM);
l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST;
l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST;
} else if (l2cap_connection_request_control_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nHID Control Incoming Connection Request"));
Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80);
#endif
pBtd->l2cap_connection_response(hci_handle,identifier, control_dcid, control_scid, PENDING);
delay(1);
@ -719,12 +719,12 @@ void WII::Run() {
pBtd->l2cap_config_request(hci_handle,identifier, control_scid);
l2cap_state = L2CAP_CONTROL_SUCCESS;
}
break;
break;
case L2CAP_CHECK_MOTION_PLUS_STATE:
#ifdef DEBUG
if(stateCounter == 0) // Only print onnce
Notify(PSTR("\r\nChecking if a Motion Plus is connected"));
Notify(PSTR("\r\nChecking if a Motion Plus is connected"), 0x80);
#endif
stateCounter++;
if(stateCounter%200 == 0)
@ -733,28 +733,28 @@ void WII::Run() {
stateCounter = 0;
l2cap_state = L2CAP_INIT_MOTION_PLUS_STATE;
timer = micros();
if(unknownExtensionConnected) {
#ifdef DEBUG
Notify(PSTR("\r\nA extension is also connected"));
Notify(PSTR("\r\nA extension is also connected"), 0x80);
#endif
activateNunchuck = true; // For we will just set this to true as this the only extension supported so far
}
}
else if(stateCounter == 601) { // We will try three times to check for the motion plus
#ifdef DEBUG
Notify(PSTR("\r\nNo Motion Plus was detected"));
#ifdef DEBUG
Notify(PSTR("\r\nNo Motion Plus was detected"), 0x80);
#endif
stateCounter = 0;
l2cap_state = L2CAP_CHECK_EXTENSION_STATE;
}
break;
case L2CAP_CHECK_EXTENSION_STATE: // This is used to check if there is anything plugged in to the extension port
#ifdef DEBUG
if(stateCounter == 0) // Only print onnce
Notify(PSTR("\r\nChecking if there is any extension connected"));
Notify(PSTR("\r\nChecking if there is any extension connected"), 0x80);
#endif
stateCounter++; // We use this counter as there has to be a short delay between the commands
if(stateCounter == 1)
@ -774,7 +774,7 @@ void WII::Run() {
l2cap_state = L2CAP_LED_STATE;
}
break;
case L2CAP_INIT_MOTION_PLUS_STATE:
stateCounter++;
if(stateCounter == 1)
@ -784,24 +784,24 @@ void WII::Run() {
else if(stateCounter == 200)
readExtensionType(); // Check if it has been activated
else if(stateCounter == 300) {
stateCounter = 0;
stateCounter = 0;
unknownExtensionConnected = false; // The motion plus will send a status report when it's activated, we will set this to false so it doesn't reinitialize the Motion Plus
l2cap_state = L2CAP_LED_STATE;
}
break;
case L2CAP_LED_STATE:
if(nunchuck_connected_flag)
nunchuckConnected = true;
setLedStatus();
l2cap_state = L2CAP_DONE;
break;
case L2CAP_DONE:
if(unknownExtensionConnected) {
#ifdef DEBUG
if(stateCounter == 0) // Only print once
Notify(PSTR("\r\nChecking extension port"));
Notify(PSTR("\r\nChecking extension port"), 0x80);
#endif
stateCounter++; // We will use this counter as there has to be a short delay between the commands
if(stateCounter == 50)
@ -818,7 +818,7 @@ void WII::Run() {
else if(stateCounter == 250) {
if(nunchuck_connected_flag) {
#ifdef DEBUG
Notify(PSTR("\r\nNunchuck was reconnected"));
Notify(PSTR("\r\nNunchuck was reconnected"), 0x80);
#endif
activateNunchuck = true;
nunchuckConnected = true;
@ -829,7 +829,7 @@ void WII::Run() {
else if (stateCounter == 300) {
if(motionPlusConnected) {
#ifdef DEBUG
Notify(PSTR("\r\nReactivating the Motion Plus"));
Notify(PSTR("\r\nReactivating the Motion Plus"), 0x80);
#endif
initMotionPlus();
} else
@ -867,7 +867,7 @@ void WII::setAllOff() {
void WII::setRumbleOff() {
HIDBuffer[1] = 0x11;
HIDBuffer[2] &= ~0x01; // Bit 0 control the rumble
HID_Command(HIDBuffer, 3);
HID_Command(HIDBuffer, 3);
}
void WII::setRumbleOn() {
HIDBuffer[1] = 0x11;
@ -963,12 +963,12 @@ void WII::activateMotionPlus() {
uint8_t buf[1];
if(pBtd->wiiUProController) {
#ifdef DEBUG
Notify(PSTR("\r\nActivating Wii U Pro Controller"));
Notify(PSTR("\r\nActivating Wii U Pro Controller"), 0x80);
#endif
buf[0] = 0x00; // It seems like you can send anything but 0x04, 0x05, and 0x07
} else if(activateNunchuck) {
#ifdef DEBUG
Notify(PSTR("\r\nActivating Motion Plus in pass-through mode"));
Notify(PSTR("\r\nActivating Motion Plus in pass-through mode"), 0x80);
#endif
buf[0] = 0x05; // Activate nunchuck pass-through mode
}
@ -976,7 +976,7 @@ void WII::activateMotionPlus() {
//buf[0] = 0x07;
else {
#ifdef DEBUG
Notify(PSTR("\r\nActivating Motion Plus in normal mode"));
Notify(PSTR("\r\nActivating Motion Plus in normal mode"), 0x80);
#endif
buf[0] = 0x04; // Don't use any extension
}
@ -989,13 +989,13 @@ void WII::readData(uint32_t offset, uint16_t size, bool EEPROM) {
if(EEPROM)
cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Read from EEPROM
else
cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Read from memory
cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Read from memory
cmd_buf[3] = (uint8_t)((offset & 0xFF0000) >> 16);
cmd_buf[4] = (uint8_t)((offset & 0xFF00) >> 8);
cmd_buf[5] = (uint8_t)(offset & 0xFF);
cmd_buf[6] = (uint8_t)((size & 0xFF00) >> 8);
cmd_buf[7] = (uint8_t)(size & 0xFF);
HID_Command(cmd_buf,8);
}
void WII::readExtensionType() {
@ -1014,7 +1014,7 @@ void WII::checkMotionPresent() {
bool WII::getButtonPress(Button b) { // Return true when a button is pressed
if(wiiUProControllerConnected)
return (ButtonState & pgm_read_dword(&PROCONTROLLERBUTTONS[(uint8_t)b]));
return (ButtonState & pgm_read_dword(&PROCONTROLLERBUTTONS[(uint8_t)b]));
else
return (ButtonState & pgm_read_dword(&BUTTONS[(uint8_t)b]));
}
@ -1057,69 +1057,69 @@ uint16_t WII::getAnalogHat(AnalogHat a) {
#ifdef WIICAMERA
void WII::IRinitialize(){ // Turns on and initialises the IR camera
enableIRCamera1();
#ifdef DEBUG
Notify(PSTR("\r\nEnable IR Camera1 Complete"));
Notify(PSTR("\r\nEnable IR Camera1 Complete"), 0x80);
#endif
delay(80);
enableIRCamera2();
#ifdef DEBUG
Notify(PSTR("\r\nEnable IR Camera2 Complete"));
Notify(PSTR("\r\nEnable IR Camera2 Complete"), 0x80);
#endif
delay(80);
write0x08Value();
#ifdef DEBUG
Notify(PSTR("\r\nWrote hex number 0x08"));
Notify(PSTR("\r\nWrote hex number 0x08"), 0x80);
#endif
delay(80);
writeSensitivityBlock1();
#ifdef DEBUG
Notify(PSTR("\r\nWrote Sensitivity Block 1"));
Notify(PSTR("\r\nWrote Sensitivity Block 1"), 0x80);
#endif
delay(80);
writeSensitivityBlock2();
#ifdef DEBUG
Notify(PSTR("\r\nWrote Sensitivity Block 2"));
Notify(PSTR("\r\nWrote Sensitivity Block 2"), 0x80);
#endif
delay(80);
uint8_t mode_num = 0x03;
setWiiModeNumber(mode_num); // Change input for whatever mode you want i.e. 0x01, 0x03, or 0x05
#ifdef DEBUG
Notify(PSTR("\r\nSet Wii Mode Number To 0x"));
PrintHex<uint8_t>(mode_num);
Notify(PSTR("\r\nSet Wii Mode Number To 0x"), 0x80);
PrintHex<uint8_t>(mode_num, 0x80);
#endif
delay(80);
write0x08Value();
#ifdef DEBUG
Notify(PSTR("\r\nWrote Hex Number 0x08"));
Notify(PSTR("\r\nWrote Hex Number 0x08"), 0x80);
#endif
delay(80);
setReportMode(false, 0x33);
//setReportMode(false, 0x3f); // For full reporting mode, doesn't work yet
#ifdef DEBUG
Notify(PSTR("\r\nSet Report Mode to 0x33"));
Notify(PSTR("\r\nSet Report Mode to 0x33"), 0x80);
#endif
delay(80);
statusRequest(); // Used to update wiiState - call isIRCameraEnabled() afterwards to check if it actually worked
#ifdef DEBUG
Notify(PSTR("\r\nIR Initialized"));
#endif
Notify(PSTR("\r\nIR Initialized"), 0x80);
#endif
}
void WII::enableIRCamera1(){
uint8_t cmd_buf[3];
cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
cmd_buf[1] = 0x13; // Output report 13
cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2
cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2
HID_Command(cmd_buf, 3);
}
@ -1127,7 +1127,7 @@ void WII::enableIRCamera2(){
uint8_t cmd_buf[3];
cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
cmd_buf[1] = 0x1A; // Output report 1A
cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2
cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2
HID_Command(cmd_buf, 3);
}
@ -1162,4 +1162,4 @@ void WII::write0x08Value(){
void WII::setWiiModeNumber(uint8_t mode_number){ // mode_number in hex i.e. 0x03 for extended mode
writeData(0xb00033,1,&mode_number);
}
#endif
#endif

178
XBOXRECV.cpp
View file

@ -1,15 +1,15 @@
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
@ -29,10 +29,10 @@ bPollEnable(false) { // don't start polling before dongle is connected
for(uint8_t i=0; i<XBOX_MAX_ENDPOINTS; i++) {
epInfo[i].epAddr = 0;
epInfo[i].maxPktSize = (i) ? 0 : 8;
epInfo[i].epAttribs = 0;
epInfo[i].epAttribs = 0;
epInfo[i].bmNakPower = (i) ? USB_NAK_NOWAIT : USB_NAK_MAX_POWER;
}
if (pUsb) // register in USB subsystem
pUsb->RegisterDeviceClass(this); //set devConfig[] entry
}
@ -44,74 +44,74 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
EpInfo *oldep_ptr = NULL;
uint16_t PID;
uint16_t VID;
// get memory address of USB device address pool
AddressPool &addrPool = pUsb->GetAddressPool();
AddressPool &addrPool = pUsb->GetAddressPool();
#ifdef EXTRADEBUG
Notify(PSTR("\r\nXBOXRECV Init"));
Notify(PSTR("\r\nXBOXRECV Init"), 0x80);
#endif
// check if address has already been assigned to an instance
if (bAddress) {
#ifdef DEBUG
Notify(PSTR("\r\nAddress in use"));
Notify(PSTR("\r\nAddress in use"), 0x80);
#endif
return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
}
// Get pointer to pseudo device with address 0 assigned
p = addrPool.GetUsbDevicePtr(0);
if (!p) {
if (!p) {
#ifdef DEBUG
Notify(PSTR("\r\nAddress not found"));
Notify(PSTR("\r\nAddress not found"), 0x80);
#endif
return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
}
if (!p->epinfo) {
#ifdef DEBUG
Notify(PSTR("\r\nepinfo is null"));
Notify(PSTR("\r\nepinfo is null"), 0x80);
#endif
return USB_ERROR_EPINFO_IS_NULL;
}
// Save old pointer to EP_RECORD of address 0
oldep_ptr = p->epinfo;
// Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence
p->epinfo = epInfo;
p->lowspeed = lowspeed;
// Get device descriptor
rcode = pUsb->getDevDescr(0, 0, sizeof(USB_DEVICE_DESCRIPTOR), (uint8_t*)buf);// Get device descriptor - addr, ep, nbytes, data
// Restore p->epinfo
p->epinfo = oldep_ptr;
if(rcode)
goto FailGetDevDescr;
VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor;
PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct;
if(VID != XBOX_VID && VID != MADCATZ_VID) // We just check if it's a xbox receiver using the Vendor ID
goto FailUnknownDevice;
else if(PID != XBOX_WIRELESS_RECEIVER_PID && PID != XBOX_WIRELESS_RECEIVER_THIRD_PARTY_PID) {
#ifdef DEBUG
Notify(PSTR("\r\nYou'll need a wireless receiver for this libary to work"));
Notify(PSTR("\r\nYou'll need a wireless receiver for this libary to work"), 0x80);
#endif
goto FailUnknownDevice;
}
}
// Allocate new address according to device class
bAddress = addrPool.AllocAddress(parent, false, port);
if (!bAddress)
return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
// Extract Max Packet Size from device descriptor
epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
// Assign new address to the device
rcode = pUsb->setAddr( 0, 0, bAddress );
if (rcode) {
@ -119,33 +119,33 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
addrPool.FreeAddress(bAddress);
bAddress = 0;
#ifdef DEBUG
Notify(PSTR("\r\nsetAddr: "));
Notify(PSTR("\r\nsetAddr: "), 0x80);
#endif
PrintHex<uint8_t>(rcode);
PrintHex<uint8_t>(rcode, 0x80);
return rcode;
}
#ifdef EXTRADEBUG
Notify(PSTR("\r\nAddr: "));
PrintHex<uint8_t>(bAddress);
Notify(PSTR("\r\nAddr: "), 0x80);
PrintHex<uint8_t>(bAddress, 0x80);
#endif
p->lowspeed = false;
//get pointer to assigned address record
p = addrPool.GetUsbDevicePtr(bAddress);
if (!p)
if (!p)
return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
p->lowspeed = lowspeed;
p->lowspeed = lowspeed;
// Assign epInfo to epinfo pointer - only EP0 is known
rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
if (rcode)
goto FailSetDevTblEntry;
/* The application will work in reduced host mode, so we can save program and data
memory space. After verifying the VID we will use known values for the
configuration values for device, interface, endpoints and HID for the XBOX360 Wireless receiver */
/* Initialize data structures for endpoints of device */
epInfo[ XBOX_INPUT_PIPE_1 ].epAddr = 0x01; // XBOX 360 report endpoint - poll interval 1ms
epInfo[ XBOX_INPUT_PIPE_1 ].epAttribs = EP_INTERRUPT;
@ -159,7 +159,7 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
epInfo[ XBOX_OUTPUT_PIPE_1 ].maxPktSize = EP_MAXPKTSIZE;
epInfo[ XBOX_OUTPUT_PIPE_1 ].bmSndToggle = bmSNDTOG0;
epInfo[ XBOX_OUTPUT_PIPE_1 ].bmRcvToggle = bmRCVTOG0;
epInfo[ XBOX_INPUT_PIPE_2 ].epAddr = 0x03; // XBOX 360 report endpoint - poll interval 1ms
epInfo[ XBOX_INPUT_PIPE_2 ].epAttribs = EP_INTERRUPT;
epInfo[ XBOX_INPUT_PIPE_2 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
@ -172,7 +172,7 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
epInfo[ XBOX_OUTPUT_PIPE_2 ].maxPktSize = EP_MAXPKTSIZE;
epInfo[ XBOX_OUTPUT_PIPE_2 ].bmSndToggle = bmSNDTOG0;
epInfo[ XBOX_OUTPUT_PIPE_2 ].bmRcvToggle = bmRCVTOG0;
epInfo[ XBOX_INPUT_PIPE_3 ].epAddr = 0x05; // XBOX 360 report endpoint - poll interval 1ms
epInfo[ XBOX_INPUT_PIPE_3 ].epAttribs = EP_INTERRUPT;
epInfo[ XBOX_INPUT_PIPE_3 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
@ -185,7 +185,7 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
epInfo[ XBOX_OUTPUT_PIPE_3 ].maxPktSize = EP_MAXPKTSIZE;
epInfo[ XBOX_OUTPUT_PIPE_3 ].bmSndToggle = bmSNDTOG0;
epInfo[ XBOX_OUTPUT_PIPE_3 ].bmRcvToggle = bmRCVTOG0;
epInfo[ XBOX_INPUT_PIPE_4 ].epAddr = 0x07; // XBOX 360 report endpoint - poll interval 1ms
epInfo[ XBOX_INPUT_PIPE_4 ].epAttribs = EP_INTERRUPT;
epInfo[ XBOX_INPUT_PIPE_4 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
@ -198,54 +198,54 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
epInfo[ XBOX_OUTPUT_PIPE_4 ].maxPktSize = EP_MAXPKTSIZE;
epInfo[ XBOX_OUTPUT_PIPE_4 ].bmSndToggle = bmSNDTOG0;
epInfo[ XBOX_OUTPUT_PIPE_4 ].bmRcvToggle = bmRCVTOG0;
rcode = pUsb->setEpInfoEntry(bAddress, 9, epInfo);
if( rcode )
goto FailSetDevTblEntry;
delay(200);//Give time for address change
rcode = pUsb->setConf(bAddress, epInfo[ XBOX_CONTROL_PIPE ].epAddr, 1);
if( rcode )
goto FailSetConf;
goto FailSetConf;
#ifdef DEBUG
Notify(PSTR("\r\nXbox Wireless Receiver Connected\r\n"));
Notify(PSTR("\r\nXbox Wireless Receiver Connected\r\n"), 0x80);
#endif
XboxReceiverConnected = true;
bPollEnable = true;
return 0; // successful configuration
/* diagnostic messages */
/* diagnostic messages */
FailGetDevDescr:
#ifdef DEBUG
Notify(PSTR("\r\ngetDevDescr:"));
Notify(PSTR("\r\ngetDevDescr:"), 0x80);
#endif
goto Fail;
goto Fail;
FailSetDevTblEntry:
#ifdef DEBUG
Notify(PSTR("\r\nsetDevTblEn:"));
Notify(PSTR("\r\nsetDevTblEn:"), 0x80);
#endif
goto Fail;
FailSetConf:
#ifdef DEBUG
Notify(PSTR("\r\nsetConf:"));
Notify(PSTR("\r\nsetConf:"), 0x80);
#endif
goto Fail;
goto Fail;
FailUnknownDevice:
#ifdef DEBUG
Notify(PSTR("\r\nUnknown Device Connected - VID: "));
PrintHex<uint16_t>(VID);
Notify(PSTR(" PID: "));
PrintHex<uint16_t>(PID);
Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80);
PrintHex<uint16_t>(VID, 0x80);
Notify(PSTR(" PID: "), 0x80);
PrintHex<uint16_t>(PID, 0x80);
#endif
rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
goto Fail;
Fail:
#ifdef DEBUG
Notify(PSTR("\r\nXbox 360 Init Failed, error code: "));
Notify(PSTR("\r\nXbox 360 Init Failed, error code: "), 0x80);
Serial.print(rcode,HEX);
#endif
#endif
Release();
return rcode;
}
@ -255,12 +255,12 @@ uint8_t XBOXRECV::Release() {
XboxReceiverConnected = false;
for(uint8_t i=0;i<4;i++)
Xbox360Connected[i] = 0x00;
pUsb->GetAddressPool().FreeAddress(bAddress);
pUsb->GetAddressPool().FreeAddress(bAddress);
bAddress = 0;
bPollEnable = false;
return 0;
}
uint8_t XBOXRECV::Poll() {
uint8_t XBOXRECV::Poll() {
if (!bPollEnable)
return 0;
if(!timer || ((millis() - timer) > 3000)) { // Run checkStatus every 3 seconds
@ -269,7 +269,7 @@ uint8_t XBOXRECV::Poll() {
}
uint8_t inputPipe;
uint16_t bufferSize;
for(uint8_t i=0;i<4;i++) {
for(uint8_t i=0;i<4;i++) {
switch (i) {
case 0: inputPipe = XBOX_INPUT_PIPE_1; break;
case 1: inputPipe = XBOX_INPUT_PIPE_2; break;
@ -280,14 +280,14 @@ uint8_t XBOXRECV::Poll() {
pUsb->inTransfer(bAddress, epInfo[ inputPipe ].epAddr, &bufferSize, readBuf);
if(bufferSize > 0) { // The number of received bytes
#ifdef EXTRADEBUG
Notify(PSTR("Bytes Received: "));
Notify(PSTR("Bytes Received: "), 0x80);
Serial.print(bufferSize);
Notify(PSTR("\r\n"));
Notify(PSTR("\r\n"), 0x80);
#endif
readReport(i);
#ifdef PRINTREPORT
printReport(i,bufferSize); // Uncomment "#define PRINTREPORT" to print the report send by the Xbox 360 Controller
#endif
#endif
}
}
return 0;
@ -300,7 +300,7 @@ void XBOXRECV::readReport(uint8_t controller) {
if(readBuf[0] == 0x08 && readBuf[1] != Xbox360Connected[controller]) {
Xbox360Connected[controller] = readBuf[1];
#ifdef DEBUG
Notify(PSTR("Controller "));
Notify(PSTR("Controller "), 0x80);
Serial.print(controller);
#endif
if(Xbox360Connected[controller]) {
@ -310,9 +310,9 @@ void XBOXRECV::readReport(uint8_t controller) {
case 0x80: str = PSTR(" as controller\r\n"); break;
case 0x40: str = PSTR(" as headset\r\n"); break;
case 0xC0: str = PSTR(" as controller+headset\r\n"); break;
}
Notify(PSTR(": connected"));
Notify(str);
}
Notify(PSTR(": connected"), 0x80);
Notify(str, 0x80);
#endif
LED led;
switch (controller) {
@ -325,7 +325,7 @@ void XBOXRECV::readReport(uint8_t controller) {
}
#ifdef DEBUG
else
Notify(PSTR(": disconnected\r\n"));
Notify(PSTR(": disconnected\r\n"), 0x80);
#endif
return;
}
@ -336,21 +336,21 @@ void XBOXRECV::readReport(uint8_t controller) {
}
if(readBuf[1] != 0x01) // Check if it's the correct report - the receiver also sends different status reports
return;
// A controller must be connected if it's sending data
if(!Xbox360Connected[controller])
Xbox360Connected[controller] |= 0x80;
ButtonState[controller] = (uint32_t)(readBuf[9] | ((uint16_t)readBuf[8] << 8) | ((uint32_t)readBuf[7] << 16) | ((uint32_t)readBuf[6] << 24));
hatValue[controller][LeftHatX] = (int16_t)(((uint16_t)readBuf[11] << 8) | readBuf[10]);
hatValue[controller][LeftHatY] = (int16_t)(((uint16_t)readBuf[13] << 8) | readBuf[12]);
hatValue[controller][RightHatX] = (int16_t)(((uint16_t)readBuf[15] << 8) | readBuf[14]);
hatValue[controller][RightHatY] = (int16_t)(((uint16_t)readBuf[17] << 8) | readBuf[16]);
//Notify(PSTR("\r\nButtonState: "));
//PrintHex<uint32_t>(ButtonState[controller]);
//Notify(PSTR("\r\nButtonState: "), 0x80);
//PrintHex<uint32_t>(ButtonState[controller], 0x80);
if(ButtonState[controller] != OldButtonState[controller]) {
buttonStateChanged[controller] = true;
ButtonClickState[controller] = (ButtonState[controller] >> 16) & ((~OldButtonState[controller]) >> 16); // Update click state variable, but don't include the two trigger buttons L2 and R2
@ -366,11 +366,11 @@ void XBOXRECV::printReport(uint8_t controller, uint8_t nBytes) { //Uncomment "#d
#ifdef PRINTREPORT
if (readBuf == NULL)
return;
Notify(PSTR("Controller "));
Notify(PSTR("Controller "), 0x80);
Serial.print(controller);
Notify(PSTR(": "));
Notify(PSTR(": "), 0x80);
for(uint8_t i = 0; i < nBytes;i++) {
PrintHex<uint8_t>(readBuf[i]);
PrintHex<uint8_t>(readBuf[i], 0x80);
Serial.print(" ");
}
Serial.println();
@ -433,8 +433,8 @@ ControllerStatus Breakdown
uint8_t XBOXRECV::getBatteryLevel(uint8_t controller) {
uint8_t batteryLevel = ((controllerStatus[controller] & 0x00C0) >> 6) * 33;
if(batteryLevel == 99)
batteryLevel = 100;
return batteryLevel;
batteryLevel = 100;
return batteryLevel;
}
void XBOXRECV::XboxCommand(uint8_t controller, uint8_t* data, uint16_t nbytes) {
@ -449,7 +449,7 @@ void XBOXRECV::XboxCommand(uint8_t controller, uint8_t* data, uint16_t nbytes) {
rcode = pUsb->outTransfer(bAddress, epInfo[ outputPipe ].epAddr, nbytes, data);
#ifdef EXTRADEBUG
if(rcode)
Notify(PSTR("Error sending Xbox message\r\n"));
Notify(PSTR("Error sending Xbox message\r\n"), 0x80);
#endif
}
void XBOXRECV::setLedRaw(uint8_t controller, uint8_t value) {
@ -457,7 +457,7 @@ void XBOXRECV::setLedRaw(uint8_t controller, uint8_t value) {
writeBuf[1] = 0x00;
writeBuf[2] = 0x08;
writeBuf[3] = value | 0x40;
XboxCommand(controller, writeBuf, 4);
}
void XBOXRECV::setLedOn(uint8_t controller, LED led) {
@ -470,12 +470,12 @@ void XBOXRECV::setLedBlink(uint8_t controller, LED led) {
void XBOXRECV::setLedMode(uint8_t controller, LEDMode ledMode) { // This function is used to do some speciel LED stuff the controller supports
setLedRaw(controller,(uint8_t)ledMode);
}
/* PC runs this at interval of approx 2 seconds
/* PC runs this at interval of approx 2 seconds
Thanks to BusHound from Perisoft.net for the Windows USB Analysis output
Found by timstamp.co.uk
*/
void XBOXRECV::checkStatus() {
if(!bPollEnable)
if(!bPollEnable)
return;
// Get controller info
writeBuf[0] = 0x08;
@ -504,6 +504,6 @@ void XBOXRECV::setRumbleOn(uint8_t controller, uint8_t lValue, uint8_t rValue) {
writeBuf[4] = 0x00;
writeBuf[5] = lValue; // big weight
writeBuf[6] = rValue; // small weight
XboxCommand(controller, writeBuf, 7);
}
}

150
XBOXUSB.cpp
View file

@ -1,15 +1,15 @@
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
@ -27,10 +27,10 @@ bPollEnable(false) { // don't start polling before dongle is connected
for(uint8_t i=0; i<XBOX_MAX_ENDPOINTS; i++) {
epInfo[i].epAddr = 0;
epInfo[i].maxPktSize = (i) ? 0 : 8;
epInfo[i].epAttribs = 0;
epInfo[i].epAttribs = 0;
epInfo[i].bmNakPower = (i) ? USB_NAK_NOWAIT : USB_NAK_MAX_POWER;
}
if (pUsb) // register in USB subsystem
pUsb->RegisterDeviceClass(this); //set devConfig[] entry
}
@ -42,80 +42,80 @@ uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
EpInfo *oldep_ptr = NULL;
uint16_t PID;
uint16_t VID;
// get memory address of USB device address pool
AddressPool &addrPool = pUsb->GetAddressPool();
AddressPool &addrPool = pUsb->GetAddressPool();
#ifdef EXTRADEBUG
Notify(PSTR("\r\nXBOXUSB Init"));
Notify(PSTR("\r\nXBOXUSB Init"), 0x80);
#endif
// check if address has already been assigned to an instance
if (bAddress) {
#ifdef DEBUG
Notify(PSTR("\r\nAddress in use"));
Notify(PSTR("\r\nAddress in use"), 0x80);
#endif
return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
}
// Get pointer to pseudo device with address 0 assigned
p = addrPool.GetUsbDevicePtr(0);
if (!p) {
if (!p) {
#ifdef DEBUG
Notify(PSTR("\r\nAddress not found"));
Notify(PSTR("\r\nAddress not found"), 0x80);
#endif
return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
}
if (!p->epinfo) {
#ifdef DEBUG
Notify(PSTR("\r\nepinfo is null"));
Notify(PSTR("\r\nepinfo is null"), 0x80);
#endif
return USB_ERROR_EPINFO_IS_NULL;
}
// Save old pointer to EP_RECORD of address 0
oldep_ptr = p->epinfo;
// Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence
p->epinfo = epInfo;
p->lowspeed = lowspeed;
// Get device descriptor
rcode = pUsb->getDevDescr(0, 0, sizeof(USB_DEVICE_DESCRIPTOR), (uint8_t*)buf);// Get device descriptor - addr, ep, nbytes, data
// Restore p->epinfo
p->epinfo = oldep_ptr;
if(rcode)
goto FailGetDevDescr;
VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor;
PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct;
if(VID != XBOX_VID && VID != MADCATZ_VID && VID != JOYTECH_VID) // We just check if it's a xbox controller using the Vendor ID
goto FailUnknownDevice;
if(PID == XBOX_WIRELESS_PID) {
#ifdef DEBUG
Notify(PSTR("\r\nYou have plugged in a wireless Xbox 360 controller - it doesn't support USB communication"));
Notify(PSTR("\r\nYou have plugged in a wireless Xbox 360 controller - it doesn't support USB communication"), 0x80);
#endif
goto FailUnknownDevice;
}
else if(PID == XBOX_WIRELESS_RECEIVER_PID || PID == XBOX_WIRELESS_RECEIVER_THIRD_PARTY_PID) {
#ifdef DEBUG
Notify(PSTR("\r\nThis library only supports Xbox 360 controllers via USB"));
Notify(PSTR("\r\nThis library only supports Xbox 360 controllers via USB"), 0x80);
#endif
goto FailUnknownDevice;
}
}
// Allocate new address according to device class
bAddress = addrPool.AllocAddress(parent, false, port);
if (!bAddress)
return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
// Extract Max Packet Size from device descriptor
epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
// Assign new address to the device
rcode = pUsb->setAddr( 0, 0, bAddress );
if (rcode) {
@ -123,33 +123,33 @@ uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
addrPool.FreeAddress(bAddress);
bAddress = 0;
#ifdef DEBUG
Notify(PSTR("\r\nsetAddr: "));
Notify(PSTR("\r\nsetAddr: "), 0x80);
#endif
PrintHex<uint8_t>(rcode);
PrintHex<uint8_t>(rcode, 0x80);
return rcode;
}
#ifdef EXTRADEBUG
Notify(PSTR("\r\nAddr: "));
PrintHex<uint8_t>(bAddress);
Notify(PSTR("\r\nAddr: "), 0x80);
PrintHex<uint8_t>(bAddress, 0x80);
#endif
p->lowspeed = false;
//get pointer to assigned address record
p = addrPool.GetUsbDevicePtr(bAddress);
if (!p)
if (!p)
return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
p->lowspeed = lowspeed;
p->lowspeed = lowspeed;
// Assign epInfo to epinfo pointer - only EP0 is known
rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
if (rcode)
goto FailSetDevTblEntry;
/* The application will work in reduced host mode, so we can save program and data
memory space. After verifying the VID we will use known values for the
configuration values for device, interface, endpoints and HID for the XBOX360 Controllers */
/* Initialize data structures for endpoints of device */
epInfo[ XBOX_INPUT_PIPE ].epAddr = 0x01; // XBOX 360 report endpoint
epInfo[ XBOX_INPUT_PIPE ].epAttribs = EP_INTERRUPT;
@ -163,55 +163,55 @@ uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
epInfo[ XBOX_OUTPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE;
epInfo[ XBOX_OUTPUT_PIPE ].bmSndToggle = bmSNDTOG0;
epInfo[ XBOX_OUTPUT_PIPE ].bmRcvToggle = bmRCVTOG0;
rcode = pUsb->setEpInfoEntry(bAddress, 3, epInfo);
if( rcode )
goto FailSetDevTblEntry;
delay(200);//Give time for address change
rcode = pUsb->setConf(bAddress, epInfo[ XBOX_CONTROL_PIPE ].epAddr, 1);
if( rcode )
goto FailSetConf;
goto FailSetConf;
#ifdef DEBUG
Notify(PSTR("\r\nXbox 360 Controller Connected\r\n"));
#endif
Notify(PSTR("\r\nXbox 360 Controller Connected\r\n"), 0x80);
#endif
setLedOn(LED1);
Xbox360Connected = true;
bPollEnable = true;
return 0; // successful configuration
/* diagnostic messages */
/* diagnostic messages */
FailGetDevDescr:
#ifdef DEBUG
Notify(PSTR("\r\ngetDevDescr:"));
Notify(PSTR("\r\ngetDevDescr:"), 0x80);
#endif
goto Fail;
goto Fail;
FailSetDevTblEntry:
#ifdef DEBUG
Notify(PSTR("\r\nsetDevTblEn:"));
Notify(PSTR("\r\nsetDevTblEn:"), 0x80);
#endif
goto Fail;
FailSetConf:
#ifdef DEBUG
Notify(PSTR("\r\nsetConf:"));
Notify(PSTR("\r\nsetConf:"), 0x80);
#endif
goto Fail;
goto Fail;
FailUnknownDevice:
#ifdef DEBUG
Notify(PSTR("\r\nUnknown Device Connected - VID: "));
PrintHex<uint16_t>(VID);
Notify(PSTR(" PID: "));
PrintHex<uint16_t>(PID);
Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80);
PrintHex<uint16_t>(VID, 0x80);
Notify(PSTR(" PID: "), 0x80);
PrintHex<uint16_t>(PID, 0x80);
#endif
rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
goto Fail;
Fail:
#ifdef DEBUG
Notify(PSTR("\r\nXbox 360 Init Failed, error code: "));
Notify(PSTR("\r\nXbox 360 Init Failed, error code: "), 0x80);
Serial.print(rcode,HEX);
#endif
#endif
Release();
return rcode;
}
@ -219,12 +219,12 @@ Fail:
/* Performs a cleanup after failed Init() attempt */
uint8_t XBOXUSB::Release() {
Xbox360Connected = false;
pUsb->GetAddressPool().FreeAddress(bAddress);
pUsb->GetAddressPool().FreeAddress(bAddress);
bAddress = 0;
bPollEnable = false;
return 0;
}
uint8_t XBOXUSB::Poll() {
uint8_t XBOXUSB::Poll() {
if (!bPollEnable)
return 0;
uint16_t BUFFER_SIZE = EP_MAXPKTSIZE;
@ -236,7 +236,7 @@ uint8_t XBOXUSB::Poll() {
return 0;
}
void XBOXUSB::readReport() {
void XBOXUSB::readReport() {
if (readBuf == NULL)
return;
if(readBuf[0] != 0x00 || readBuf[1] != 0x14) { // Check if it's the correct report - the controller also sends different status reports
@ -244,15 +244,15 @@ void XBOXUSB::readReport() {
}
ButtonState = (uint32_t)(readBuf[5] | ((uint16_t)readBuf[4] << 8) | ((uint32_t)readBuf[3] << 16) | ((uint32_t)readBuf[2] << 24));
hatValue[LeftHatX] = (int16_t)(((uint16_t)readBuf[7] << 8) | readBuf[6]);
hatValue[LeftHatY] = (int16_t)(((uint16_t)readBuf[9] << 8) | readBuf[8]);
hatValue[RightHatX] = (int16_t)(((uint16_t)readBuf[11] << 8) | readBuf[10]);
hatValue[RightHatY] = (int16_t)(((uint16_t)readBuf[13] << 8) | readBuf[12]);
//Notify(PSTR("\r\nButtonState"));
//PrintHex<uint32_t>(ButtonState);
//Notify(PSTR("\r\nButtonState"), 0x80);
//PrintHex<uint32_t>(ButtonState, 0x80);
if(ButtonState != OldButtonState) {
ButtonClickState = (ButtonState >> 16) & ((~OldButtonState) >> 16); // Update click state variable, but don't include the two trigger buttons L2 and R2
if(((uint8_t)OldButtonState) == 0 && ((uint8_t)ButtonState) != 0) // The L2 and R2 buttons are special as they are analog buttons
@ -261,18 +261,18 @@ void XBOXUSB::readReport() {
L2Clicked = true;
OldButtonState = ButtonState;
}
}
}
void XBOXUSB::printReport() { //Uncomment "#define PRINTREPORT" to print the report send by the Xbox 360 Controller
#ifdef PRINTREPORT
if (readBuf == NULL)
return;
for(uint8_t i = 0; i < XBOX_REPORT_BUFFER_SIZE;i++) {
PrintHex<uint8_t>(readBuf[i]);
PrintHex<uint8_t>(readBuf[i], 0x80);
Serial.print(" ");
}
}
Serial.println();
#endif
#endif
}
uint8_t XBOXUSB::getButtonPress(Button b) {
@ -315,7 +315,7 @@ void XBOXUSB::setLedRaw(uint8_t value) {
writeBuf[0] = 0x01;
writeBuf[1] = 0x03;
writeBuf[2] = value;
XboxCommand(writeBuf, 3);
}
void XBOXUSB::setLedOn(LED led) {
@ -337,6 +337,6 @@ void XBOXUSB::setRumbleOn(uint8_t lValue, uint8_t rValue) {
writeBuf[5] = 0x00;
writeBuf[6] = 0x00;
writeBuf[7] = 0x00;
XboxCommand(writeBuf, 8);
}
}