Added HCI Version Check

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[5]) { // check if command succeeded
-                if((hcibuf[3] == 0x09) && (hcibuf[4] == 0x10))// parameters from read local bluetooth address
+                    hci_event_flag |= HCI_FLAG_CMD_COMPLETE; // set command complete flag
-                {  
+                    if((hcibuf[3] == 0x01) && (hcibuf[4] == 0x10)) // parameters from read local version information
-                    for (uint8_t i = 0; i < 6; i++) 
+                        hci_version = hcibuf[6]; // Check if it supports 2.0+EDR - see http://www.bluetooth.org/Technical/AssignedNumbers/hci.htm
-                        my_bdaddr[i] = hcibuf[6 + i];
+                    
+                    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;
                 
@@ -685,9 +683,6 @@ void PS3BT::HCI_event_task()
                 }
                 break;                              
                 
-            case EV_NUM_COMPLETE_PKT:
-                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)
             {
@@ -793,10 +793,27 @@ void PS3BT::HCI_task()
                     Serial.print(":");
                 }      
                 PrintHex<uint8_t>(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
             Notify(PSTR("\r\nWait For Incoming Connection Request"));
@@ -1320,6 +1337,13 @@ void PS3BT::hci_read_bdaddr()
     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()
 {    
     hcibuf[0] = 0x09; // HCI OCF = 9

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_LOCAL_VERSION_STATE 3
-#define HCI_CONNECT_IN_STATE    4
+#define HCI_SCANNING_STATE      4
-#define HCI_REMOTE_NAME_STATE   5
+#define HCI_CONNECT_IN_STATE    5
-#define HCI_CONNECTED_STATE     6
+#define HCI_REMOTE_NAME_STATE   6
-#define HCI_DISABLE_SCAN        7
+#define HCI_CONNECTED_STATE     7
-#define HCI_DONE_STATE          8
+#define HCI_DISABLE_SCAN        8
-#define HCI_DISCONNECT_STATE    9
+#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();