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

Merge pull request #14 from TKJElectronics/master

Browse source 
Added 360 degrees resolution of angle calculated from accelerometer
This commit is contained in:
Oleg Mazurov 2012-04-24 13:53:11 -07:00
commit 92e25ae896
3 changed files with 174 additions and 185 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

309
PS3BT.cpp
View file

@ -24,8 +24,7 @@ const uint8_t PS3BT::BTD_EVENT_PIPE = 1;
const uint8_t PS3BT::BTD_DATAIN_PIPE = 2; const uint8_t PS3BT::BTD_DATAIN_PIPE = 2;
const uint8_t PS3BT::BTD_DATAOUT_PIPE = 3; const uint8_t PS3BT::BTD_DATAOUT_PIPE = 3;
prog_char OUTPUT_REPORT_BUFFER[] PROGMEM = prog_char OUTPUT_REPORT_BUFFER[] PROGMEM = {
{
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,
@ -37,7 +36,7 @@ prog_char OUTPUT_REPORT_BUFFER[] PROGMEM =
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
}; };
PS3BT::PS3BT(USB *p, uint8_t btadr5 = 0, uint8_t btadr4 = 0, uint8_t btadr3 = 0, uint8_t btadr2 = 0, uint8_t btadr1 = 0, uint8_t btadr0 = 0): PS3BT::PS3BT(USB *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0):
pUsb(p), // pointer to USB class instance - mandatory pUsb(p), // pointer to USB class instance - mandatory
bAddress(0), // device address - mandatory bAddress(0), // device address - mandatory
bNumEP(1), // if config descriptor needs to be parsed bNumEP(1), // if config descriptor needs to be parsed
@ -94,15 +93,15 @@ uint8_t PS3BT::Init(uint8_t parent, uint8_t port, bool lowspeed)
// 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\nPS3BT Init")); Notify(PSTR("\r\nPS3BT Init"));
#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"));
#endif #endif
return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
} }
@ -111,17 +110,17 @@ uint8_t PS3BT::Init(uint8_t parent, uint8_t port, bool lowspeed)
if (!p) if (!p)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nAddress not found")); Notify(PSTR("\r\nAddress not found"));
#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"));
#endif #endif
return USB_ERROR_EPINFO_IS_NULL; return USB_ERROR_EPINFO_IS_NULL;
} }
@ -158,16 +157,16 @@ uint8_t PS3BT::Init(uint8_t parent, uint8_t port, bool lowspeed)
p->lowspeed = false; p->lowspeed = false;
addrPool.FreeAddress(bAddress); addrPool.FreeAddress(bAddress);
bAddress = 0; bAddress = 0;
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nsetAddr: ")); Notify(PSTR("\r\nsetAddr: "));
#endif #endif
PrintHex<uint8_t>(rcode); PrintHex<uint8_t>(rcode);
return rcode; return rcode;
} }
#ifdef EXTRADEBUG #ifdef EXTRADEBUG
Notify(PSTR("\r\nAddr: ")); Notify(PSTR("\r\nAddr: "));
PrintHex<uint8_t>(bAddress); PrintHex<uint8_t>(bAddress);
#endif #endif
p->lowspeed = false; p->lowspeed = false;
//get pointer to assigned address record //get pointer to assigned address record
@ -218,22 +217,22 @@ uint8_t PS3BT::Init(uint8_t parent, uint8_t port, bool lowspeed)
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"));
#endif #endif
} 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"));
#endif #endif
} }
/* Set internal bluetooth address */ /* Set internal bluetooth address */
setBdaddr(my_bdaddr); setBdaddr(my_bdaddr);
} }
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"));
#endif #endif
setMoveBdaddr(my_bdaddr); setMoveBdaddr(my_bdaddr);
} }
} }
@ -289,49 +288,49 @@ uint8_t PS3BT::Init(uint8_t parent, uint8_t port, bool lowspeed)
hci_state = HCI_INIT_STATE; hci_state = HCI_INIT_STATE;
hci_counter = 0; hci_counter = 0;
l2cap_state = L2CAP_EV_WAIT; l2cap_state = L2CAP_EV_WAIT;
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nBluetooth Dongle Initialized")); Notify(PSTR("\r\nBluetooth Dongle Initialized"));
#endif #endif
watingForConnection = false; watingForConnection = false;
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:"));
#endif #endif
goto Fail; goto Fail;
FailSetDevTblEntry: FailSetDevTblEntry:
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nsetDevTblEn:")); Notify(PSTR("\r\nsetDevTblEn:"));
#endif #endif
goto Fail; goto Fail;
FailGetConfDescr: FailGetConfDescr:
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\ngetConf:")); Notify(PSTR("\r\ngetConf:"));
#endif #endif
goto Fail; goto Fail;
FailSetConf: FailSetConf:
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nsetConf:")); Notify(PSTR("\r\nsetConf:"));
#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: "));
PrintHex<uint16_t>(VID); PrintHex<uint16_t>(VID);
Notify(PSTR(" PID: ")); Notify(PSTR(" PID: "));
PrintHex<uint16_t>(PID); PrintHex<uint16_t>(PID);
#endif #endif
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: "));
Serial.print(rcode); Serial.print(rcode);
#endif #endif
Release(); Release();
return rcode; return rcode;
} }
@ -361,9 +360,9 @@ void PS3BT::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t pro
//Fill the rest of endpoint data structure //Fill the rest of endpoint data structure
epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F); epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F);
epInfo[index].maxPktSize = (uint8_t)pep->wMaxPacketSize; epInfo[index].maxPktSize = (uint8_t)pep->wMaxPacketSize;
#ifdef EXTRADEBUG #ifdef EXTRADEBUG
PrintEndpointDescriptor(pep); PrintEndpointDescriptor(pep);
#endif #endif
if(pollInterval < pep->bInterval) // Set the polling interval as the largest polling interval obtained from endpoints if(pollInterval < pep->bInterval) // Set the polling interval as the largest polling interval obtained from endpoints
pollInterval = pep->bInterval; pollInterval = pep->bInterval;
bNumEP++; bNumEP++;
@ -421,7 +420,7 @@ void PS3BT::setBdaddr(uint8_t* BDADDR)
//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: "));
for(int8_t i = 5; i > 0; i--) for(int8_t i = 5; i > 0; i--)
{ {
@ -429,7 +428,7 @@ void PS3BT::setBdaddr(uint8_t* BDADDR)
Serial.print(":"); Serial.print(":");
} }
PrintHex<uint8_t>(my_bdaddr[0]); PrintHex<uint8_t>(my_bdaddr[0]);
#endif #endif
return; return;
} }
void PS3BT::setMoveBdaddr(uint8_t* BDADDR) void PS3BT::setMoveBdaddr(uint8_t* BDADDR)
@ -451,7 +450,7 @@ void PS3BT::setMoveBdaddr(uint8_t* BDADDR)
//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: "));
for(int8_t i = 5; i > 0; i--) for(int8_t i = 5; i > 0; i--)
{ {
@ -459,7 +458,7 @@ void PS3BT::setMoveBdaddr(uint8_t* BDADDR)
Serial.print(":"); Serial.print(":");
} }
PrintHex<uint8_t>(my_bdaddr[0]); PrintHex<uint8_t>(my_bdaddr[0]);
#endif #endif
return; return;
} }
bool PS3BT::getButton(Button b) bool PS3BT::getButton(Button b)
@ -483,7 +482,7 @@ uint8_t PS3BT::getAnalogHat(AnalogHat a)
return 0; return 0;
return (uint8_t)(l2capinbuf[(uint16_t)a]); return (uint8_t)(l2capinbuf[(uint16_t)a]);
} }
uint32_t PS3BT::getSensor(Sensor a) int32_t PS3BT::getSensor(Sensor a)
{ {
if (l2capinbuf == NULL) if (l2capinbuf == NULL)
return 0; return 0;
@ -505,83 +504,63 @@ uint32_t PS3BT::getSensor(Sensor a)
return 0; return 0;
} }
else if (a == tempMove) else if (a == tempMove)
{
if (l2capinbuf == NULL)
return 0;
return (((l2capinbuf[(uint16_t)a + 1] & 0xF0) >> 4) | (l2capinbuf[(uint16_t)a] << 4)); return (((l2capinbuf[(uint16_t)a + 1] & 0xF0) >> 4) | (l2capinbuf[(uint16_t)a] << 4));
} else // aXmove, aYmove, aZmove, gXmove, gYmove and gZmove
else return ((l2capinbuf[(uint16_t)a + 1] << 8) | l2capinbuf[(uint16_t)a]);
{
return (((l2capinbuf[(uint16_t)a + 1] << 8) | l2capinbuf[(uint16_t)a]) - 0x8000);
}
} }
double PS3BT::getAngle(Angle a, boolean resolution) // Boolean indicate if 360-degrees resolution is used or not - set false if you want to use both axis double PS3BT::getAngle(Angle a) {
{
double accXin;
double accXval; double accXval;
double angleX;
double accYin;
double accYval; double accYval;
double angleY;
double accZin;
double accZval; double accZval;
//Data for the Kionix KXPC4 used in DualShock 3 if(PS3BTConnected) {
double sensivity = 204.6;//0.66/3.3*1023 (660mV/g) // Data for the Kionix KXPC4 used in the DualShock 3
double zeroG = 511.5;//1.65/3.3*1023 (1,65V) double sensivity = 204.6; // 0.66/3.3*1023 (660mV/g)
double R;//force vector double zeroG = 511.5; // 1.65/3.3*1023 (1,65V)
accXval = ((double)getSensor(aX)-zeroG) / sensivity; // Convert to g's
accXin = getSensor(aX);
accXval = (zeroG - accXin) / sensivity;//Convert to g's
accXval *= 2; accXval *= 2;
accYval = ((double)getSensor(aY)-zeroG) / sensivity; // Convert to g's
accYin = getSensor(aY);
accYval = (zeroG - accYin) / sensivity;//Convert to g's
accYval *= 2; accYval *= 2;
accZval = ((double)getSensor(aZ)-zeroG) / sensivity; // Convert to g's
accZin = getSensor(aZ);
accZval = (zeroG - accZin) / sensivity;//Convert to g's
accZval *= 2; accZval *= 2;
} else if(PS3MoveBTConnected) {
// It's a Kionix KXSC4 inside the Motion controller
const uint16_t sensivity = 28285; // Find by experimenting
accXval = (double)getSensor(aXmove)/sensivity;
accYval = (double)getSensor(aYmove)/sensivity;
accZval = (double)getSensor(aZmove)/sensivity;
R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2)); if(accXval < -1) // Convert to g's
accXval = ((1+accXval)-(1-1.15))*(-1/0.15);
else if(accXval > 1)
accXval = ((1+accXval)-(1+1.15))*(-1/0.15);
if (a == Pitch) if(accYval < -1) // Convert to g's
{ accYval = ((1+accYval)-(1-1.15))*(-1/0.15);
//the result will come out as radians, so it is multiplied by 180/pi, to convert to degrees else if(accYval > 1)
//In the end it is minus by 90, so its 0 degrees when in horizontal postion accYval = ((1+accYval)-(1+1.15))*(-1/0.15);
angleX = acos(accXval / R) * 180 / PI - 90;
if(resolution)
{
if (accZval < 0)//Convert to 360 degrees resolution - set resolution false if you need both pitch and roll
{
if (angleX < 0)
angleX = -180 - angleX;
else
angleX = 180 - angleX;
}
}
return angleX;
if(accZval < -1) // Convert to g's
accZval = ((1+accZval)-(1-1.15))*(-1/0.15);
else if(accZval > 1)
accZval = ((1+accZval)-(1+1.15))*(-1/0.15);
} }
else
{ double R = sqrt(accXval*accXval + accYval*accYval + accZval*accZval); // Calculate the length of the force vector
//the result will come out as radians, so it is multiplied by 180/pi, to convert to degrees // Normalize vectors
//In the end it is minus by 90, so its 0 degrees when in horizontal postion accXval = accXval/R;
angleY = acos(accYval / R) * 180 / PI - 90; accYval = accYval/R;
if(resolution) accZval = accZval/R;
{
if (accZval < 0)//Convert to 360 degrees resolution - set resolution false if you need both pitch and roll // Convert to 360 degrees resolution
{ // atan2 outputs the value of -π to π (radians)
if (angleY < 0) // We are then converting it to 0 to 2π and then to degrees
angleY = -180 - angleY; if (a == Pitch) {
else double angle = (atan2(-accYval,-accZval)+PI)*RAD_TO_DEG;
angleY = 180 - angleY; return angle;
} } else {
} double angle = (atan2(-accXval,-accZval)+PI)*RAD_TO_DEG;
return angleY; return angle;
} }
} }
bool PS3BT::getStatus(Status c) bool PS3BT::getStatus(Status c)
@ -679,14 +658,14 @@ void PS3BT::HCI_event_task()
case EV_COMMAND_STATUS: case EV_COMMAND_STATUS:
if(hcibuf[2]) // show status on serial if not OK if(hcibuf[2]) // show status on serial if not OK
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nHCI Command Failed: ")); Notify(PSTR("\r\nHCI Command Failed: "));
PrintHex<uint8_t>(hcibuf[2]); PrintHex<uint8_t>(hcibuf[2]);
Serial.print(" "); Serial.print(" ");
PrintHex<uint8_t>(hcibuf[4]); PrintHex<uint8_t>(hcibuf[4]);
Serial.print(" "); Serial.print(" ");
PrintHex<uint8_t>(hcibuf[5]); PrintHex<uint8_t>(hcibuf[5]);
#endif #endif
} }
break; break;
@ -748,22 +727,22 @@ void PS3BT::HCI_event_task()
break; break;
default: default:
#ifdef EXTRADEBUG #ifdef EXTRADEBUG
if(hcibuf[0] != 0x00) if(hcibuf[0] != 0x00)
{ {
Notify(PSTR("\r\nUnmanaged Event: ")); Notify(PSTR("\r\nUnmanaged Event: "));
PrintHex<uint8_t>(hcibuf[0]); PrintHex<uint8_t>(hcibuf[0]);
} }
#endif #endif
break; break;
} // switch } // switch
HCI_task(); HCI_task();
} }
else { else {
#ifdef EXTRADEBUG #ifdef EXTRADEBUG
Notify(PSTR("\r\nHCI event error: ")); Notify(PSTR("\r\nHCI event error: "));
PrintHex<uint8_t>(rcode); PrintHex<uint8_t>(rcode);
#endif #endif
} }
} }
@ -785,9 +764,9 @@ void PS3BT::HCI_task()
hci_counter++; hci_counter++;
if (hci_cmd_complete) if (hci_cmd_complete)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nHCI Reset complete")); Notify(PSTR("\r\nHCI Reset complete"));
#endif #endif
hci_state = HCI_BDADDR_STATE; hci_state = HCI_BDADDR_STATE;
hci_read_bdaddr(); hci_read_bdaddr();
} }
@ -796,9 +775,9 @@ void PS3BT::HCI_task()
hci_num_reset_loops *= 10; hci_num_reset_loops *= 10;
if(hci_num_reset_loops > 2000) if(hci_num_reset_loops > 2000)
hci_num_reset_loops = 2000; hci_num_reset_loops = 2000;
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nNo response to HCI Reset")); Notify(PSTR("\r\nNo response to HCI Reset"));
#endif #endif
hci_state = HCI_INIT_STATE; hci_state = HCI_INIT_STATE;
hci_counter = 0; hci_counter = 0;
} }
@ -806,7 +785,7 @@ void PS3BT::HCI_task()
case HCI_BDADDR_STATE: case HCI_BDADDR_STATE:
if (hci_cmd_complete) if (hci_cmd_complete)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nLocal Bluetooth Address: ")); Notify(PSTR("\r\nLocal Bluetooth Address: "));
for(int8_t i = 5; i > 0;i--) for(int8_t i = 5; i > 0;i--)
{ {
@ -814,14 +793,14 @@ void PS3BT::HCI_task()
Serial.print(":"); Serial.print(":");
} }
PrintHex<uint8_t>(my_bdaddr[0]); PrintHex<uint8_t>(my_bdaddr[0]);
#endif #endif
hci_state = HCI_SCANNING_STATE; hci_state = HCI_SCANNING_STATE;
} }
break; break;
case HCI_SCANNING_STATE: case HCI_SCANNING_STATE:
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nWait For Incoming Connection Request")); Notify(PSTR("\r\nWait For Incoming Connection Request"));
#endif #endif
hci_write_scan_enable(); hci_write_scan_enable();
watingForConnection = true; watingForConnection = true;
hci_state = HCI_CONNECT_IN_STATE; hci_state = HCI_CONNECT_IN_STATE;
@ -831,9 +810,9 @@ void PS3BT::HCI_task()
if(hci_incoming_connect_request) if(hci_incoming_connect_request)
{ {
watingForConnection = false; watingForConnection = false;
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nIncoming Request")); Notify(PSTR("\r\nIncoming Request"));
#endif #endif
hci_remote_name(); hci_remote_name();
hci_state = HCI_REMOTE_NAME_STATE; hci_state = HCI_REMOTE_NAME_STATE;
} }
@ -842,7 +821,7 @@ void PS3BT::HCI_task()
case HCI_REMOTE_NAME_STATE: case HCI_REMOTE_NAME_STATE:
if(hci_remote_name_complete) if(hci_remote_name_complete)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nRemote Name: ")); Notify(PSTR("\r\nRemote Name: "));
for (uint8_t i = 0; i < 30; i++) for (uint8_t i = 0; i < 30; i++)
{ {
@ -850,7 +829,7 @@ void PS3BT::HCI_task()
break; break;
Serial.write(remote_name[i]); Serial.write(remote_name[i]);
} }
#endif #endif
hci_accept_connection(); hci_accept_connection();
hci_state = HCI_CONNECTED_STATE; hci_state = HCI_CONNECTED_STATE;
} }
@ -859,7 +838,7 @@ void PS3BT::HCI_task()
case HCI_CONNECTED_STATE: case HCI_CONNECTED_STATE:
if (hci_connect_complete) if (hci_connect_complete)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nConnected to Device: ")); Notify(PSTR("\r\nConnected to Device: "));
for(int8_t i = 5; i>0;i--) for(int8_t i = 5; i>0;i--)
{ {
@ -867,7 +846,7 @@ void PS3BT::HCI_task()
Serial.print(":"); Serial.print(":");
} }
PrintHex<uint8_t>(disc_bdaddr[0]); PrintHex<uint8_t>(disc_bdaddr[0]);
#endif #endif
hci_write_scan_disable(); // Only allow one controller hci_write_scan_disable(); // Only allow one controller
hci_state = HCI_DISABLE_SCAN; hci_state = HCI_DISABLE_SCAN;
} }
@ -876,9 +855,9 @@ void PS3BT::HCI_task()
case HCI_DISABLE_SCAN: case HCI_DISABLE_SCAN:
if (hci_cmd_complete) if (hci_cmd_complete)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nScan Disabled")); Notify(PSTR("\r\nScan Disabled"));
#endif #endif
l2cap_event_flag = 0; l2cap_event_flag = 0;
l2cap_state = L2CAP_EV_CONTROL_SETUP; l2cap_state = L2CAP_EV_CONTROL_SETUP;
hci_state = HCI_DONE_STATE; hci_state = HCI_DONE_STATE;
@ -893,7 +872,7 @@ void PS3BT::HCI_task()
case HCI_DISCONNECT_STATE: case HCI_DISCONNECT_STATE:
if (hci_disconnect_complete) if (hci_disconnect_complete)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nDisconnected from Device: ")); Notify(PSTR("\r\nDisconnected from Device: "));
for(int8_t i = 5; i>0;i--) for(int8_t i = 5; i>0;i--)
{ {
@ -901,7 +880,7 @@ void PS3BT::HCI_task()
Serial.print(":"); Serial.print(":");
} }
PrintHex<uint8_t>(disc_bdaddr[0]); PrintHex<uint8_t>(disc_bdaddr[0]);
#endif #endif
l2cap_event_flag = 0; // Clear all flags l2cap_event_flag = 0; // Clear all flags
hci_event_flag = 0; // Clear all flags hci_event_flag = 0; // Clear all flags
@ -946,7 +925,7 @@ void PS3BT::ACL_event_task()
*/ */
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: "));
PrintHex<uint8_t>(l2capinbuf[13]); PrintHex<uint8_t>(l2capinbuf[13]);
Serial.print(" "); Serial.print(" ");
@ -959,7 +938,7 @@ void PS3BT::ACL_event_task()
PrintHex<uint8_t>(l2capinbuf[15]); PrintHex<uint8_t>(l2capinbuf[15]);
Serial.print(" "); Serial.print(" ");
PrintHex<uint8_t>(l2capinbuf[14]); PrintHex<uint8_t>(l2capinbuf[14]);
#endif #endif
} }
else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST)
{ {
@ -1031,17 +1010,17 @@ void PS3BT::ACL_event_task()
{ {
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\nDisconnected Request: Disconnected Control")); Notify(PSTR("\r\nDisconnected Request: Disconnected Control"));
#endif #endif
identifier = l2capinbuf[9]; identifier = l2capinbuf[9];
l2cap_disconnection_response(identifier,control_dcid,control_scid); l2cap_disconnection_response(identifier,control_dcid,control_scid);
} }
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\nDisconnected Request: Disconnected Interrupt")); Notify(PSTR("\r\nDisconnected Request: Disconnected Interrupt"));
#endif #endif
identifier = l2capinbuf[9]; identifier = l2capinbuf[9];
l2cap_disconnection_response(identifier,interrupt_dcid,interrupt_scid); l2cap_disconnection_response(identifier,interrupt_dcid,interrupt_scid);
} }
@ -1068,19 +1047,19 @@ void PS3BT::ACL_event_task()
if(PS3BTConnected || PS3MoveBTConnected || PS3NavigationBTConnected) if(PS3BTConnected || PS3MoveBTConnected || PS3NavigationBTConnected)
{ {
readReport(); readReport();
#ifdef PRINTREPORT #ifdef PRINTREPORT
printReport(); //Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers printReport(); //Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers
#endif #endif
} }
} }
L2CAP_task(); L2CAP_task();
} }
} }
else { else {
#ifdef EXTRADEBUG #ifdef EXTRADEBUG
Notify(PSTR("\r\nACL data in error: ")); Notify(PSTR("\r\nACL data in error: "));
PrintHex<uint8_t>(rcode); PrintHex<uint8_t>(rcode);
#endif #endif
} }
} }
void PS3BT::L2CAP_task() void PS3BT::L2CAP_task()
@ -1092,9 +1071,9 @@ void PS3BT::L2CAP_task()
case L2CAP_EV_CONTROL_SETUP: case L2CAP_EV_CONTROL_SETUP:
if (l2cap_control_connection_request) if (l2cap_control_connection_request)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nHID Control Incoming Connection Request")); Notify(PSTR("\r\nHID Control Incoming Connection Request"));
#endif #endif
l2cap_connection_response(identifier, control_dcid, control_scid, PENDING); l2cap_connection_response(identifier, control_dcid, control_scid, PENDING);
delay(1); delay(1);
l2cap_connection_response(identifier, control_dcid, control_scid, SUCCESSFUL); l2cap_connection_response(identifier, control_dcid, control_scid, SUCCESSFUL);
@ -1107,9 +1086,9 @@ void PS3BT::L2CAP_task()
case L2CAP_EV_CONTROL_REQUEST: case L2CAP_EV_CONTROL_REQUEST:
if (l2cap_control_config_request) if (l2cap_control_config_request)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nHID Control Configuration Request")); Notify(PSTR("\r\nHID Control Configuration Request"));
#endif #endif
l2cap_config_response(identifier, control_scid); l2cap_config_response(identifier, control_scid);
l2cap_state = L2CAP_EV_CONTROL_SUCCESS; l2cap_state = L2CAP_EV_CONTROL_SUCCESS;
} }
@ -1118,18 +1097,18 @@ void PS3BT::L2CAP_task()
case L2CAP_EV_CONTROL_SUCCESS: case L2CAP_EV_CONTROL_SUCCESS:
if (l2cap_control_config_success) if (l2cap_control_config_success)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nHID Control Successfully Configured")); Notify(PSTR("\r\nHID Control Successfully Configured"));
#endif #endif
l2cap_state = L2CAP_EV_INTERRUPT_SETUP; l2cap_state = L2CAP_EV_INTERRUPT_SETUP;
} }
break; break;
case L2CAP_EV_INTERRUPT_SETUP: case L2CAP_EV_INTERRUPT_SETUP:
if (l2cap_interrupt_connection_request) if (l2cap_interrupt_connection_request)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nHID Interrupt Incoming Connection Request")); Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"));
#endif #endif
l2cap_connection_response(identifier, interrupt_dcid, interrupt_scid, PENDING); l2cap_connection_response(identifier, interrupt_dcid, interrupt_scid, PENDING);
delay(1); delay(1);
l2cap_connection_response(identifier, interrupt_dcid, interrupt_scid, SUCCESSFUL); l2cap_connection_response(identifier, interrupt_dcid, interrupt_scid, SUCCESSFUL);
@ -1143,9 +1122,9 @@ void PS3BT::L2CAP_task()
case L2CAP_EV_INTERRUPT_REQUEST: case L2CAP_EV_INTERRUPT_REQUEST:
if (l2cap_interrupt_config_request) if (l2cap_interrupt_config_request)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nHID Interrupt Configuration Request")); Notify(PSTR("\r\nHID Interrupt Configuration Request"));
#endif #endif
l2cap_config_response(identifier, interrupt_scid); l2cap_config_response(identifier, interrupt_scid);
l2cap_state = L2CAP_EV_INTERRUPT_SUCCESS; l2cap_state = L2CAP_EV_INTERRUPT_SUCCESS;
} }
@ -1153,9 +1132,9 @@ void PS3BT::L2CAP_task()
case L2CAP_EV_INTERRUPT_SUCCESS: case L2CAP_EV_INTERRUPT_SUCCESS:
if (l2cap_interrupt_config_success) if (l2cap_interrupt_config_success)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nHID Interrupt Successfully Configured")); Notify(PSTR("\r\nHID Interrupt Successfully Configured"));
#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;
@ -1187,22 +1166,22 @@ void PS3BT::L2CAP_task()
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"));
#endif #endif
PS3BTConnected = true; PS3BTConnected = 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"));
#endif #endif
PS3NavigationBTConnected = true; PS3NavigationBTConnected = 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"));
#endif #endif
PS3MoveBTConnected = true; PS3MoveBTConnected = true;
} }
l2cap_state = L2CAP_EV_L2CAP_DONE; l2cap_state = L2CAP_EV_L2CAP_DONE;
@ -1224,9 +1203,9 @@ void PS3BT::L2CAP_task()
case L2CAP_EV_INTERRUPT_DISCONNECT: case L2CAP_EV_INTERRUPT_DISCONNECT:
if (l2cap_interrupt_disconnect_response) if (l2cap_interrupt_disconnect_response)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nDisconnected Interrupt Channel")); Notify(PSTR("\r\nDisconnected Interrupt Channel"));
#endif #endif
identifier++; identifier++;
l2cap_disconnection_request(identifier, control_dcid, control_scid); l2cap_disconnection_request(identifier, control_dcid, control_scid);
l2cap_state = L2CAP_EV_CONTROL_DISCONNECT; l2cap_state = L2CAP_EV_CONTROL_DISCONNECT;
@ -1236,9 +1215,9 @@ void PS3BT::L2CAP_task()
case L2CAP_EV_CONTROL_DISCONNECT: case L2CAP_EV_CONTROL_DISCONNECT:
if (l2cap_control_disconnect_response) if (l2cap_control_disconnect_response)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nDisconnected Control Channel")); Notify(PSTR("\r\nDisconnected Control Channel"));
#endif #endif
hci_disconnect(); hci_disconnect();
l2cap_state = L2CAP_EV_L2CAP_DONE; l2cap_state = L2CAP_EV_L2CAP_DONE;
hci_state = HCI_DISCONNECT_STATE; hci_state = HCI_DISCONNECT_STATE;
@ -1408,10 +1387,10 @@ void PS3BT::L2CAP_Command(uint8_t* data, uint16_t nbytes)
uint8_t rcode = pUsb->outTransfer(bAddress, epInfo[ BTD_DATAOUT_PIPE ].epAddr, (8 + nbytes), buf); uint8_t rcode = pUsb->outTransfer(bAddress, epInfo[ BTD_DATAOUT_PIPE ].epAddr, (8 + nbytes), buf);
if(rcode) if(rcode)
{ {
#ifdef DEBUG #ifdef DEBUG
Notify(PSTR("\r\nError sending message: 0x")); Notify(PSTR("\r\nError sending message: 0x"));
PrintHex(rcode); PrintHex(rcode);
#endif #endif
} }
} }
void PS3BT::l2cap_connection_response(uint8_t rxid, uint8_t dcid[], uint8_t scid[], uint8_t result) void PS3BT::l2cap_connection_response(uint8_t rxid, uint8_t dcid[], uint8_t scid[], uint8_t result)

4
PS3BT.h
View file

@ -338,8 +338,8 @@ public:
bool getButton(Button b); bool getButton(Button b);
uint8_t getAnalogButton(AnalogButton a); uint8_t getAnalogButton(AnalogButton a);
uint8_t getAnalogHat(AnalogHat a); uint8_t getAnalogHat(AnalogHat a);
uint32_t getSensor(Sensor a); int32_t getSensor(Sensor a);
double getAngle(Angle a, boolean resolution); double getAngle(Angle a);
bool getStatus(Status c); bool getStatus(Status c);
String getStatusString(); String getStatusString();
void disconnect(); // use this void to disconnect any of the controllers void disconnect(); // use this void to disconnect any of the controllers

20
examples/PS3BT/PS3BT.ino
View file

@ -123,9 +123,9 @@ void loop()
} }
if(printAngle) { if(printAngle) {
Serial.print(F("Pitch: ")); Serial.print(F("Pitch: "));
Serial.print(BT.getAngle(Pitch,false)); Serial.print(BT.getAngle(Pitch));
Serial.print(F("\tRoll: ")); Serial.print(F("\tRoll: "));
Serial.println(BT.getAngle(Roll,false)); Serial.println(BT.getAngle(Roll));
} }
} }
else if(BT.PS3MoveBTConnected) else if(BT.PS3MoveBTConnected)
@ -142,10 +142,14 @@ void loop()
} else { } else {
if(BT.getButton(SELECT_MOVE)) { if(BT.getButton(SELECT_MOVE)) {
Serial.print(F(" - Select")); Serial.print(F(" - Select"));
printTemperature = false; printTemperature = !printTemperature;
while(BT.getButton(SELECT_MOVE))
Usb.Task();
} if(BT.getButton(START_MOVE)) { } if(BT.getButton(START_MOVE)) {
Serial.print(F(" - Start")); Serial.print(F(" - Start"));
printTemperature = true; printAngle = !printAngle;
while(BT.getButton(START_MOVE))
Usb.Task();
} if(BT.getButton(TRIANGLE_MOVE)) { } if(BT.getButton(TRIANGLE_MOVE)) {
Serial.print(F(" - Triangle")); Serial.print(F(" - Triangle"));
BT.moveSetBulb(Red); BT.moveSetBulb(Red);
@ -170,7 +174,13 @@ void loop()
Serial.println(""); Serial.println("");
} }
} }
if(printTemperature) { if(printAngle) {
Serial.print(F("Pitch: "));
Serial.print(BT.getAngle(Pitch));
Serial.print(F("\tRoll: "));
Serial.println(BT.getAngle(Roll));
}
else if(printTemperature) {
String templow; String templow;
String temphigh; String temphigh;
String input = String(BT.getSensor(tempMove)); String input = String(BT.getSensor(tempMove));