23 PS3BT::PS3BT(
BTD *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0) :
40 HIDMoveBuffer[0] = 0xA2;
41 HIDMoveBuffer[1] = 0x02;
44 control_dcid[0] = 0x40;
45 control_dcid[1] = 0x00;
46 interrupt_dcid[0] = 0x41;
47 interrupt_dcid[1] = 0x00;
53 return (ButtonState & pgm_read_dword(&
BUTTONS[(uint8_t)b]));
57 uint32_t button = pgm_read_dword(&
BUTTONS[(uint8_t)b]);
58 bool click = (ButtonClickState & button);
59 ButtonClickState &= ~button;
64 return (uint8_t)(l2capinbuf[pgm_read_byte(&
ANALOGBUTTONS[(uint8_t)a])]);
68 return (uint8_t)(l2capinbuf[(uint8_t)a + 15]);
73 if (a ==
aX || a ==
aY || a ==
aZ || a ==
gZ)
74 return ((l2capinbuf[(uint16_t)a] << 8) | l2capinbuf[(uint16_t)a + 1]);
79 return (((l2capinbuf[(uint16_t)a] & 0x0F) << 8) | (l2capinbuf[(uint16_t)a + 1]));
81 return ((l2capinbuf[(uint16_t)a] << 4) | ((l2capinbuf[(uint16_t)a + 1] & 0xF0) >> 4));
83 return (l2capinbuf[(uint16_t)a] | (l2capinbuf[(uint16_t)a + 1] << 8));
89 double accXval, accYval, accZval;
93 const double zeroG = 511.5;
99 const uint16_t zeroG = 0x8000;
110 return (atan2(accYval, accZval) + PI) * RAD_TO_DEG;
112 return (atan2(accXval, accZval) + PI) * RAD_TO_DEG;
122 return (
double)value / 3.2;
128 return (
double)value / 442.0;
135 return (
double)value / 11.6;
137 return (
double)value / 11.2;
139 return (
double)value / 9.6;
148 String output = String(input / 100);
150 if (input % 100 < 10)
152 output += String(input % 100);
160 return (l2capinbuf[(uint16_t)c >> 8] == ((uint8_t)c & 0xff));
165 char statusOutput[100];
167 strcpy(statusOutput,
"ConnectionStatus: ");
171 else strcat(statusOutput,
"Error");
174 strcat(statusOutput,
" - PowerRating: ");
182 else strcat(statusOutput,
"Error");
184 strcat(statusOutput,
" - WirelessStatus: ");
187 else if (
getStatus(
Cable)) strcat(statusOutput,
"Cable - Rumble is off");
190 else strcat(statusOutput,
"Error");
195 char statusOutput[50];
197 strcpy(statusOutput,
"PowerRating: ");
206 else strcat(statusOutput,
"Error");
217 activeConnection =
false;
218 l2cap_event_flag = 0;
236 if ((ACLData[12] | (ACLData[13] << 8)) ==
HID_CTRL_PSM) {
238 activeConnection =
true;
241 for (uint8_t i = 0; i < 30; i++)
243 #ifdef DEBUG_USB_HOST
245 Notify(PSTR(
"\r\nYour dongle may not support reading the analog buttons, sensors and status\r\nYour HCI Version is: "), 0x80);
247 Notify(PSTR(
"\r\nBut should be at least 3\r\nThis means that it doesn't support Bluetooth Version 2.0+EDR"), 0x80);
253 if (((ACLData[0] | (ACLData[1] << 8)) == (hci_handle | 0x2000))) {
255 if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) {
257 #ifdef DEBUG_USB_HOST
258 Notify(PSTR(
"\r\nL2CAP Command Rejected - Reason: "), 0x80);
259 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
261 D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
262 Notify(PSTR(
" Data: "), 0x80);
263 D_PrintHex<uint8_t > (l2capinbuf[17], 0x80);
265 D_PrintHex<uint8_t > (l2capinbuf[16], 0x80);
267 D_PrintHex<uint8_t > (l2capinbuf[15], 0x80);
269 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
273 Notify(PSTR(
"\r\nL2CAP Connection Request - PSM: "), 0x80);
274 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
276 D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
277 Notify(PSTR(
" SCID: "), 0x80);
278 D_PrintHex<uint8_t > (l2capinbuf[15], 0x80);
280 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
281 Notify(PSTR(
" Identifier: "), 0x80);
282 D_PrintHex<uint8_t > (l2capinbuf[9], 0x80);
284 if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) ==
HID_CTRL_PSM) {
285 identifier = l2capinbuf[9];
286 control_scid[0] = l2capinbuf[14];
287 control_scid[1] = l2capinbuf[15];
289 }
else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) ==
HID_INTR_PSM) {
290 identifier = l2capinbuf[9];
291 interrupt_scid[0] = l2capinbuf[14];
292 interrupt_scid[1] = l2capinbuf[15];
296 if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) {
297 if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
300 }
else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
306 if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
308 identifier = l2capinbuf[9];
310 }
else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
312 identifier = l2capinbuf[9];
316 if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
317 #ifdef DEBUG_USB_HOST
318 Notify(PSTR(
"\r\nDisconnect Request: Control Channel"), 0x80);
320 identifier = l2capinbuf[9];
323 }
else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
324 #ifdef DEBUG_USB_HOST
325 Notify(PSTR(
"\r\nDisconnect Request: Interrupt Channel"), 0x80);
327 identifier = l2capinbuf[9];
332 if (l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
334 identifier = l2capinbuf[9];
336 }
else if (l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) {
338 identifier = l2capinbuf[9];
344 Notify(PSTR(
"\r\nL2CAP Unknown Signaling Command: "), 0x80);
345 D_PrintHex<uint8_t > (l2capinbuf[8], 0x80);
348 }
else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) {
352 if (l2capinbuf[8] == 0xA1) {
354 ButtonState = (uint32_t)(l2capinbuf[11] | ((uint16_t)l2capinbuf[12] << 8) | ((uint32_t)l2capinbuf[13] << 16));
356 ButtonState = (uint32_t)(l2capinbuf[10] | ((uint16_t)l2capinbuf[11] << 8) | ((uint32_t)l2capinbuf[12] << 16));
361 if (ButtonState != OldButtonState) {
362 ButtonClickState = ButtonState & ~OldButtonState;
363 OldButtonState = ButtonState;
366 #ifdef PRINTREPORT // Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers
367 for (uint8_t i = 10; i < 58; i++) {
368 D_PrintHex<uint8_t > (l2capinbuf[i], 0x80);
371 Notify(PSTR(
"\r\n"), 0x80);
380 void PS3BT::L2CAP_task() {
381 switch (l2cap_state) {
384 #ifdef DEBUG_USB_HOST
385 Notify(PSTR(
"\r\nHID Control Incoming Connection Request"), 0x80);
398 #ifdef DEBUG_USB_HOST
399 Notify(PSTR(
"\r\nHID Control Configuration Request"), 0x80);
408 #ifdef DEBUG_USB_HOST
409 Notify(PSTR(
"\r\nHID Control Successfully Configured"), 0x80);
416 #ifdef DEBUG_USB_HOST
417 Notify(PSTR(
"\r\nHID Interrupt Incoming Connection Request"), 0x80);
431 #ifdef DEBUG_USB_HOST
432 Notify(PSTR(
"\r\nHID Interrupt Configuration Request"), 0x80);
440 #ifdef DEBUG_USB_HOST
441 Notify(PSTR(
"\r\nHID Interrupt Successfully Configured"), 0x80);
443 if (remote_name[0] ==
'M') {
456 #ifdef DEBUG_USB_HOST
457 Notify(PSTR(
"\r\nDisconnected Interrupt Channel"), 0x80);
467 #ifdef DEBUG_USB_HOST
468 Notify(PSTR(
"\r\nDisconnected Control Channel"), 0x80);
472 l2cap_event_flag = 0;
480 switch (l2cap_state) {
482 if (millis() - timer > 1000) {
484 for (uint8_t i = 15; i < 19; i++)
485 l2capinbuf[i] = 0x7F;
493 if (millis() - timer > 1000) {
494 if (remote_name[0] ==
'P') {
495 #ifdef DEBUG_USB_HOST
496 Notify(PSTR(
"\r\nDualshock 3 Controller Enabled\r\n"), 0x80);
499 }
else if (remote_name[0] ==
'N') {
500 #ifdef DEBUG_USB_HOST
501 Notify(PSTR(
"\r\nNavigation Controller Enabled\r\n"), 0x80);
504 }
else if (remote_name[0] ==
'M') {
505 timerBulbRumble = millis();
506 #ifdef DEBUG_USB_HOST
507 Notify(PSTR(
"\r\nMotion Controller Enabled\r\n"), 0x80);
513 ButtonClickState = 0;
522 if (millis() - timerBulbRumble > 4000) {
524 timerBulbRumble = millis();
537 void PS3BT::HID_Command(uint8_t* data, uint8_t nbytes) {
538 if (millis() - timerHID <= 150)
539 delay((uint32_t)(150 - (millis() - timerHID)));
540 pBtd->
L2CAP_Command(hci_handle, data, nbytes, control_scid[0], control_scid[1]);
550 HIDBuffer[11] = 0x00;
565 uint8_t power[2] = { 0xff, 0x00 };
573 void PS3BT::setRumbleOn(uint8_t rightDuration, uint8_t rightPower, uint8_t leftDuration, uint8_t leftPower) {
574 HIDBuffer[3] = rightDuration;
575 HIDBuffer[4] = rightPower;
576 HIDBuffer[5] = leftDuration;
577 HIDBuffer[6] = leftPower;
582 HIDBuffer[11] = value << 1;
587 HIDBuffer[11] &= ~((uint8_t)((pgm_read_byte(&
LEDS[(uint8_t)a]) & 0x0f) << 1));
592 HIDBuffer[11] |= (uint8_t)((pgm_read_byte(&
LEDS[(uint8_t)a]) & 0x0f) << 1);
597 HIDBuffer[11] ^= (uint8_t)((pgm_read_byte(&
LEDS[(uint8_t)a]) & 0x0f) << 1);
601 void PS3BT::enable_sixaxis() {
610 HID_Command(cmd_buf, 6);
615 void PS3BT::HIDMove_Command(uint8_t* data, uint8_t nbytes) {
616 if (millis() - timerHID <= 150)
617 delay((uint32_t)(150 - (millis() - timerHID)));
618 pBtd->
L2CAP_Command(hci_handle, data, nbytes, interrupt_scid[0], interrupt_scid[1]);
624 HIDMoveBuffer[3] = r;
625 HIDMoveBuffer[4] = g;
626 HIDMoveBuffer[5] = b;
632 moveSetBulb((uint8_t)(color >> 16), (uint8_t)(color >> 8), (uint8_t)(color));
636 #ifdef DEBUG_USB_HOST
637 if (rumble < 64 && rumble != 0)
638 Notify(PSTR(
"\r\nThe rumble value has to at least 64, or approximately 25%"), 0x80);
641 HIDMoveBuffer[7] = rumble;
646 void PS3BT::onInit() {
#define L2CAP_FLAG_CONFIG_CONTROL_REQUEST
uint8_t getAnalogHat(AnalogHat a)
bool getButtonClick(Button b)
bool PS3NavigationConnected
void l2cap_connection_response(uint16_t handle, uint8_t rxid, uint8_t *dcid, uint8_t *scid, uint8_t result)
void l2cap_disconnection_request(uint16_t handle, uint8_t rxid, uint8_t *dcid, uint8_t *scid)
#define l2cap_config_request_interrupt_flag
#define l2cap_disconnect_response_interrupt_flag
const uint8_t PS3_REPORT_BUFFER[]
#define L2CAP_INTERRUPT_REQUEST
void moveSetRumble(uint8_t rumble)
const uint8_t ANALOGBUTTONS[]
int16_t getSensor(Sensor a)
double get9DOFValues(Sensor a)
#define L2CAP_INTERRUPT_SUCCESS
#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS
#define l2cap_config_request_control_flag
#define l2cap_connection_request_control_flag
void l2cap_disconnection_response(uint16_t handle, uint8_t rxid, uint8_t *dcid, uint8_t *scid)
#define L2CAP_FLAG_CONFIG_INTERRUPT_REQUEST
#define L2CAP_CONTROL_REQUEST
#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE
#define l2cap_connection_request_interrupt_flag
#define L2CAP_INTERRUPT_DISCONNECT
void hci_disconnect(uint16_t handle)
void moveSetBulb(uint8_t r, uint8_t g, uint8_t b)
#define L2CAP_INTERRUPT_SETUP
#define l2cap_disconnect_response_control_flag
#define L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST
#define L2CAP_FLAG_CONNECTION_CONTROL_REQUEST
#define L2CAP_CMD_CONFIG_REQUEST
#define L2CAP_CMD_DISCONNECT_REQUEST
int8_t registerServiceClass(BluetoothService *pService)
bool l2capConnectionClaimed
#define L2CAP_CMD_DISCONNECT_RESPONSE
virtual void ACLData(uint8_t *ACLData)
#define L2CAP_CMD_CONFIG_RESPONSE
#define L2CAP_HID_ENABLE_SIXAXIS
#define l2cap_config_success_control_flag
#define l2cap_config_success_interrupt_flag
#define L2CAP_HID_PS3_LED
#define L2CAP_CONTROL_SUCCESS
void setRumbleOn(Rumble mode)
#define PS3_REPORT_BUFFER_SIZE
virtual void disconnect()
void L2CAP_Command(uint16_t handle, uint8_t *data, uint8_t nbytes, uint8_t channelLow=0x01, uint8_t channelHigh=0x00)
void l2cap_config_response(uint16_t handle, uint8_t rxid, uint8_t *scid)
void l2cap_config_request(uint16_t handle, uint8_t rxid, uint8_t *dcid)
#define L2CAP_CMD_CONNECTION_REQUEST
uint8_t getAnalogButton(Button a)
#define L2CAP_CONTROL_DISCONNECT
#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE
void setLedRaw(uint8_t value)
PS3BT(BTD *pBtd, uint8_t btadr5=0, uint8_t btadr4=0, uint8_t btadr3=0, uint8_t btadr2=0, uint8_t btadr1=0, uint8_t btadr0=0)
bool getButtonPress(Button b)
#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS
#define L2CAP_CMD_COMMAND_REJECT