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();