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

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

2047
BTD.cpp

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

View file

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

262
SPP.cpp
View file

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

326
Wii.cpp
View file

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

View file

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

View file

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