From 2330fd95becfb594589496eccd5f10b46187d950 Mon Sep 17 00:00:00 2001 From: Kristian Lauszus Date: Sat, 5 May 2012 20:58:16 +0200 Subject: [PATCH 1/3] Added HCI Version Check --- PS3BT.cpp | 56 +++++++++++++++++++++++++++++++++++++++---------------- PS3BT.h | 17 ++++++++++------- 2 files changed, 50 insertions(+), 23 deletions(-) diff --git a/PS3BT.cpp b/PS3BT.cpp index 92c8a640..f64138ad 100644 --- a/PS3BT.cpp +++ b/PS3BT.cpp @@ -546,12 +546,6 @@ double PS3BT::getAngle(Angle a) { accZval = ((1+accZval)-(1+1.15))*(-1/0.15); } - double R = sqrt(accXval*accXval + accYval*accYval + accZval*accZval); // Calculate the length of the force vector - // Normalize vectors - accXval = accXval/R; - accYval = accYval/R; - accZval = accZval/R; - // Convert to 360 degrees resolution // atan2 outputs the value of -π to π (radians) // We are then converting it to 0 to 2π and then to degrees @@ -647,11 +641,15 @@ void PS3BT::HCI_event_task() switch (hcibuf[0]) //switch on event type { case EV_COMMAND_COMPLETE: - hci_event_flag |= HCI_FLAG_CMD_COMPLETE; // set command complete flag - if((hcibuf[3] == 0x09) && (hcibuf[4] == 0x10))// parameters from read local bluetooth address - { - for (uint8_t i = 0; i < 6; i++) - my_bdaddr[i] = hcibuf[6 + i]; + if (!hcibuf[5]) { // check if command succeeded + hci_event_flag |= HCI_FLAG_CMD_COMPLETE; // set command complete flag + if((hcibuf[3] == 0x01) && (hcibuf[4] == 0x10)) // parameters from read local version information + hci_version = hcibuf[6]; // Check if it supports 2.0+EDR - see http://www.bluetooth.org/Technical/AssignedNumbers/hci.htm + + else if((hcibuf[3] == 0x09) && (hcibuf[4] == 0x10)) { // parameters from read local bluetooth address + for (uint8_t i = 0; i < 6; i++) + my_bdaddr[i] = hcibuf[6 + i]; + } } break; @@ -683,10 +681,7 @@ void PS3BT::HCI_event_task() hci_event_flag |= HCI_FLAG_DISCONN_COMPLETE; //set disconnect commend complete flag hci_event_flag &= ~HCI_FLAG_CONN_COMPLETE; // clear connection complete flag } - break; - - case EV_NUM_COMPLETE_PKT: - break; + break; case EV_REMOTE_NAME_COMPLETE: if (!hcibuf[2]) // check if reading is OK @@ -707,7 +702,11 @@ void PS3BT::HCI_event_task() hci_event_flag |= HCI_FLAG_INCOMING_REQUEST; break; - /* We will just ignore the following events */ + /* We will just ignore the following events */ + + case EV_NUM_COMPLETE_PKT: + break; + case EV_ROLE_CHANGED: break; @@ -782,6 +781,7 @@ void PS3BT::HCI_task() hci_counter = 0; } break; + case HCI_BDADDR_STATE: if (hci_cmd_complete) { @@ -794,8 +794,25 @@ void PS3BT::HCI_task() } PrintHex(my_bdaddr[0]); #endif + hci_read_local_version_information(); + hci_state = HCI_LOCAL_VERSION_STATE; + } + break; + + case HCI_LOCAL_VERSION_STATE: + if (hci_cmd_complete) + { +#ifdef DEBUG + if(hci_version < 3) { + Notify(PSTR("\r\nYour dongle may not support reading the analog buttons, sensors and status\r\nYour HCI Version is: ")); + Serial.print(hci_version); + Notify(PSTR("\r\nBut should be at least 3\r\nThis means that it doesn't support Bluetooth Version 2.0+EDR")); + } +#endif hci_state = HCI_SCANNING_STATE; } + break; + break; case HCI_SCANNING_STATE: #ifdef DEBUG @@ -1319,6 +1336,13 @@ void PS3BT::hci_read_bdaddr() hcibuf[1] = 0x04 << 2; // HCI OGF = 4 hcibuf[2] = 0x00; HCI_Command(hcibuf, 3); +} +void PS3BT::hci_read_local_version_information() +{ + hcibuf[0] = 0x01; // HCI OCF = 1 + hcibuf[1] = 0x04 << 2; // HCI OGF = 4 + hcibuf[2] = 0x00; + HCI_Command(hcibuf, 3); } void PS3BT::hci_accept_connection() { diff --git a/PS3BT.h b/PS3BT.h index 8c1f43df..b228a91c 100644 --- a/PS3BT.h +++ b/PS3BT.h @@ -62,13 +62,14 @@ #define HCI_INIT_STATE 0 #define HCI_RESET_STATE 1 #define HCI_BDADDR_STATE 2 -#define HCI_SCANNING_STATE 3 -#define HCI_CONNECT_IN_STATE 4 -#define HCI_REMOTE_NAME_STATE 5 -#define HCI_CONNECTED_STATE 6 -#define HCI_DISABLE_SCAN 7 -#define HCI_DONE_STATE 8 -#define HCI_DISCONNECT_STATE 9 +#define HCI_LOCAL_VERSION_STATE 3 +#define HCI_SCANNING_STATE 4 +#define HCI_CONNECT_IN_STATE 5 +#define HCI_REMOTE_NAME_STATE 6 +#define HCI_CONNECTED_STATE 7 +#define HCI_DISABLE_SCAN 8 +#define HCI_DONE_STATE 9 +#define HCI_DISCONNECT_STATE 10 /* HCI event flags*/ #define HCI_FLAG_CMD_COMPLETE 0x01 @@ -389,6 +390,7 @@ private: int16_t hci_handle; uint8_t disc_bdaddr[6]; // the bluetooth address is always 6 bytes uint8_t remote_name[30]; // first 30 chars of remote name + uint8_t hci_version; /* variables used by high level HCI task */ uint8_t hci_state; //current state of bluetooth hci connection @@ -437,6 +439,7 @@ private: void hci_write_scan_enable(); void hci_write_scan_disable(); void hci_read_bdaddr(); + void hci_read_local_version_information(); void hci_accept_connection(); void hci_remote_name(); void hci_disconnect(); From e60ddbac5a55d21d43dafabf32983414c3a58bcd Mon Sep 17 00:00:00 2001 From: Kristian Lauszus Date: Wed, 9 May 2012 18:38:38 +0200 Subject: [PATCH 2/3] Added setLedToggle() --- PS3BT.cpp | 34 ++++++++++++---------------------- PS3BT.h | 1 + keywords.txt | 1 + 3 files changed, 14 insertions(+), 22 deletions(-) diff --git a/PS3BT.cpp b/PS3BT.cpp index f64138ad..3e5f8107 100644 --- a/PS3BT.cpp +++ b/PS3BT.cpp @@ -406,17 +406,13 @@ uint8_t PS3BT::Poll() return 0; } void PS3BT::setBdaddr(uint8_t* BDADDR) -{ - /* Store the bluetooth address */ - for(uint8_t i = 0; i <6;i++) - my_bdaddr[i] = BDADDR[i]; - +{ /* Set the internal bluetooth address */ uint8_t buf[8]; buf[0] = 0x01; buf[1] = 0x00; for (uint8_t i = 0; i < 6; i++) - buf[i+2] = my_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) pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL); @@ -433,10 +429,6 @@ void PS3BT::setBdaddr(uint8_t* BDADDR) } void PS3BT::setMoveBdaddr(uint8_t* BDADDR) { - /* Store the bluetooth address */ - for(uint8_t i = 0; i <6;i++) - my_bdaddr[i] = BDADDR[i]; - /* Set the internal bluetooth address */ uint8_t buf[11]; buf[0] = 0x05; @@ -446,7 +438,7 @@ void PS3BT::setMoveBdaddr(uint8_t* BDADDR) buf[10] = 0x12; for (uint8_t i = 0; i < 6; i++) - buf[i + 1] = my_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) pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x05, 0x03, 0x00,11,11, buf, NULL); @@ -798,7 +790,7 @@ void PS3BT::HCI_task() hci_state = HCI_LOCAL_VERSION_STATE; } break; - + case HCI_LOCAL_VERSION_STATE: if (hci_cmd_complete) { @@ -1591,19 +1583,17 @@ void PS3BT::setRumbleOn(Rumble mode) } void PS3BT::setLedOff(LED a) { - //check if LED is already off - if ((uint8_t)((uint8_t)(((uint16_t)a << 1) & HIDBuffer[11])) != 0) - { - //set the LED into the write buffer - HIDBuffer[11] = (uint8_t)((uint8_t)(((uint16_t)a & 0x0f) << 1) ^ HIDBuffer[11]); - - HID_Command(HIDBuffer, HID_BUFFERSIZE); - } + HIDBuffer[11] &= ~((uint8_t)(((uint16_t)a & 0x0f) << 1)); + HID_Command(HIDBuffer, HID_BUFFERSIZE); } void PS3BT::setLedOn(LED a) { - HIDBuffer[11] = (uint8_t)((uint8_t)(((uint16_t)a & 0x0f) << 1) | HIDBuffer[11]); - + HIDBuffer[11] |= (uint8_t)(((uint16_t)a & 0x0f) << 1); + HID_Command(HIDBuffer, HID_BUFFERSIZE); +} +void PS3BT::setLedToggle(LED a) +{ + HIDBuffer[11] ^= (uint8_t)(((uint16_t)a & 0x0f) << 1); HID_Command(HIDBuffer, HID_BUFFERSIZE); } void PS3BT::enable_sixaxis()//Command used to enable the Dualshock 3 and Navigation controller to send data via USB diff --git a/PS3BT.h b/PS3BT.h index b228a91c..b2983b7c 100644 --- a/PS3BT.h +++ b/PS3BT.h @@ -352,6 +352,7 @@ public: void setRumbleOn(Rumble mode); void setLedOff(LED a); void setLedOn(LED a); + void setLedToggle(LED a); /* Commands for Motion controller only */ void moveSetBulb(uint8_t r, uint8_t g, uint8_t b);//Use this to set the Color using RGB values void moveSetBulb(Colors color);//Use this to set the Color using the predefined colors in "enum Colors" diff --git a/keywords.txt b/keywords.txt index 378fc91a..8b11fbcf 100644 --- a/keywords.txt +++ b/keywords.txt @@ -28,6 +28,7 @@ setRumbleOff KEYWORD2 setRumbleOn KEYWORD2 setLedOff KEYWORD2 setLedOn KEYWORD2 +setLedToggle KEYWORD2 moveSetBulb KEYWORD2 moveSetRumble KEYWORD2 From 95a797e7356809bed713c2dad355e5362cba4118 Mon Sep 17 00:00:00 2001 From: Kristian Lauszus Date: Wed, 9 May 2012 19:46:26 +0200 Subject: [PATCH 3/3] Updated getAngle() --- PS3BT.cpp | 54 ++++++++++++++++++++++++------------------------------ 1 file changed, 24 insertions(+), 30 deletions(-) diff --git a/PS3BT.cpp b/PS3BT.cpp index 3e5f8107..c80e58ab 100644 --- a/PS3BT.cpp +++ b/PS3BT.cpp @@ -507,45 +507,39 @@ double PS3BT::getAngle(Angle a) { if(PS3BTConnected) { // Data for the Kionix KXPC4 used in the DualShock 3 - double sensivity = 204.6; // 0.66/3.3*1023 (660mV/g) - double zeroG = 511.5; // 1.65/3.3*1023 (1,65V) - accXval = ((double)getSensor(aX)-zeroG) / sensivity; // Convert to g's - accXval *= 2; - accYval = ((double)getSensor(aY)-zeroG) / sensivity; // Convert to g's - accYval *= 2; - accZval = ((double)getSensor(aZ)-zeroG) / sensivity; // Convert to g's - 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; + const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V) + accXval = -((double)getSensor(aX)-zeroG); + accYval = -((double)getSensor(aY)-zeroG); + accZval = -((double)getSensor(aZ)-zeroG); + } else if(PS3MoveBTConnected) { + // It's a Kionix KXSC4 inside the Motion controller + const uint16_t zeroG = 0x8000; + accXval = getSensor(aXmove); + accYval = getSensor(aYmove); + accZval = getSensor(aZmove); - 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(accYval < -1) // Convert to g's - accYval = ((1+accYval)-(1-1.15))*(-1/0.15); - else if(accYval > 1) - accYval = ((1+accYval)-(1+1.15))*(-1/0.15); - - 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); + if(accXval < 0) + accXval += zeroG; + else + accXval -= zeroG; + if(accYval < 0) + accYval += zeroG; + else + accYval -= zeroG; + if(accZval < 0) + accZval += zeroG; + else + accZval -= zeroG; } // Convert to 360 degrees resolution // atan2 outputs the value of -π to π (radians) // We are then converting it to 0 to 2π and then to degrees if (a == Pitch) { - double angle = (atan2(-accYval,-accZval)+PI)*RAD_TO_DEG; + double angle = (atan2(accYval,accZval)+PI)*RAD_TO_DEG; return angle; } else { - double angle = (atan2(-accXval,-accZval)+PI)*RAD_TO_DEG; + double angle = (atan2(accXval,accZval)+PI)*RAD_TO_DEG; return angle; } }