Merge pull request #1 from KurtE/WIP2-Bluetooth

Wip2 bluetooth update for xbox

bluetooth.cpp

 			rxbuf_[index_bd],rxbuf_[index_bd+1],rxbuf_[index_bd+2],rxbuf_[index_bd+3],rxbuf_[index_bd+4],rxbuf_[index_bd+5],
 			rxbuf_[index_ps], bluetooth_class);
 		// See if we know the class 
-		if ((bluetooth_class & 0xff00) == 0x2500) {
+		if (((bluetooth_class & 0xff00) == 0x2500) || ((bluetooth_class & 0xff00) == 0x500)) {
 			DBGPrintf("      Peripheral device\n");
 			if (bluetooth_class & 0x80) DBGPrintf("        Mouse\n");
 			if (bluetooth_class & 0x40) DBGPrintf("        Keyboard\n"); 

examples/JoystickBT/JoystickBT.ino

 USBDriver *drivers[] = {&hub1, &hub2, &joystick1, &bluet, &hid1, &hid2, &hid3, &hid4, &hid5};
 
 #define CNT_DEVICES (sizeof(drivers)/sizeof(drivers[0]))
-const char * driver_names[CNT_DEVICES] = {"Hub1","Hub2", "JOY1D", "Bluet", "HID1" , "HID2", "HID3", "HID4", "HID5"};
+const char * driver_names[CNT_DEVICES] = {"Hub1", "Hub2", "JOY1D", "Bluet", "HID1" , "HID2", "HID3", "HID4", "HID5"};
 
 bool driver_active[CNT_DEVICES] = {false, false, false, false};
 
 const char * hid_driver_names[CNT_DEVICES] = {"Joystick1", "RawHid1", "RawHid2"};
 
 bool hid_driver_active[CNT_DEVICES] = {false, false, false};
-bool show_changed_only = false; 
+bool show_changed_only = false;
 bool show_raw_data = false;
 bool show_changed_data = false;
 
 uint8_t joystick_left_trigger_value = 0;
 uint8_t joystick_right_trigger_value = 0;
-uint64_t joystick_full_notify_mask = (uint64_t)-1;
+uint64_t joystick_full_notify_mask = (uint64_t) - 1;
 
 int psAxis[64];
 bool first_joystick_message = true;
-uint8_t last_bdaddr[6]={0,0,0,0,0,0};
+uint8_t last_bdaddr[6] = {0, 0, 0, 0, 0, 0};
 
 void setup()
 {
-/*  Serial4.begin( 1843200 );
-  debBegin_tt( &Serial4, LED_BUILTIN, 12);
-  debTraceShow_tt( -1, "", "", "" );
-  Serial4.println("\n" __FILE__ " " __DATE__ " " __TIME__);
-  Serial4.println("\n********\n T4 connected Serial1 *******\n");
-  Serial4.println("\n" __FILE__ " " __DATE__ " " __TIME__);
-  Serial4.println("\n********\n T4 connected Serial4 *******\n");
-*/
-Serial1.begin(1843200);
+  /*  Serial4.begin( 1843200 );
+    debBegin_tt( &Serial4, LED_BUILTIN, 12);
+    debTraceShow_tt( -1, "", "", "" );
+    Serial4.println("\n" __FILE__ " " __DATE__ " " __TIME__);
+    Serial4.println("\n********\n T4 connected Serial1 *******\n");
+    Serial4.println("\n" __FILE__ " " __DATE__ " " __TIME__);
+    Serial4.println("\n********\n T4 connected Serial4 *******\n");
+  */
+  Serial1.begin(2000000);
   while (!Serial) ; // wait for Arduino Serial Monitor
   //debTraceShow_tt( -2, "", "", "" );
   //Serial4.println("\n" __FILE__ " " __DATE__ " " __TIME__);
 void loop()
 {
   myusb.Task();
- 
+
   if (Serial.available()) {
-    int ch = Serial.read(); // get the first char. 
-    while (Serial.read() != -1) ; 
+    int ch = Serial.read(); // get the first char.
+    while (Serial.read() != -1) ;
     if ((ch == 'b') || (ch == 'B')) {
       Serial.println("Only notify on Basic Axis changes");
       joystick1.axisChangeNotifyMask(0x3ff);
         Serial.println("\n*** Show only changed fields mode ***");
       }
     }
- }
+  }
 
   for (uint8_t i = 0; i < CNT_DEVICES; i++) {
     if (*drivers[i] != driver_active[i]) {
           const uint8_t *bdaddr = bluet.myBDAddr();
           // remember it...
           Serial.printf("  BDADDR: %x:%x:%x:%x:%x:%x\n", bdaddr[0], bdaddr[1], bdaddr[2], bdaddr[3], bdaddr[4], bdaddr[5]);
-          for (uint8_t i=0;i<6;i++) last_bdaddr[i] = bdaddr[i];
+          for (uint8_t i = 0; i < 6; i++) last_bdaddr[i] = bdaddr[i];
         }
       }
     }
   }
 
   if (joystick1.available()) {
-      if (first_joystick_message) {
-        Serial.printf("*** First Joystick message %x:%x ***\n", 
-            joystick1.idVendor(), joystick1.idProduct());
-        first_joystick_message = false;
+    if (first_joystick_message) {
+      Serial.printf("*** First Joystick message %x:%x ***\n",
+                    joystick1.idVendor(), joystick1.idProduct());
+      first_joystick_message = false;
+
+      const uint8_t *psz = joystick1.manufacturer();
+      if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
+      psz = joystick1.product();
+      if (psz && *psz) Serial.printf("  product: %s\n", psz);
+      psz = joystick1.serialNumber();
+      if (psz && *psz) Serial.printf("  Serial: %s\n", psz);
+
+      // lets try to reduce number of fields that update
+      joystick1.axisChangeNotifyMask(0xFFFFFl);
+    }
 
-        const uint8_t *psz = joystick1.manufacturer();
-        if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
-        psz = joystick1.product();
-        if (psz && *psz) Serial.printf("  product: %s\n", psz);
-        psz =joystick1.serialNumber();
-        if (psz && *psz) Serial.printf("  Serial: %s\n", psz);    
+    for (uint8_t i = 0; i < 64; i++) {
+      psAxis[i] = joystick1.getAxis(i);
+    }
+    switch (joystick1.joystickType()) {
+      case JoystickController::UNKNOWN:
+      case JoystickController::PS4:
+        displayPS4Data();
+        break;
+      case JoystickController::PS3:
+        displayPS3Data();
+        break;
+      case JoystickController::XBOXONE:
+      case JoystickController::XBOX360:
+        displayXBoxData();
+        break;
+      default:
+        displayRawData();
+        break;
+    }
+    //for (uint8_t i = 0; i < 24; i++) {
+    //  Serial.printf(" %d:%d", i, psAxis[i]);
+    //}
+    //Serial.println();
 
-        // lets try to reduce number of fields that update
-         joystick1.axisChangeNotifyMask(0xFFFFFl);    
-      }
-      
-      for (uint8_t i = 0; i<64; i++) {
-          psAxis[i] = joystick1.getAxis(i);
-      }
-      switch (joystick1.joystickType()) {
-        case JoystickController::UNKNOWN:
-        case JoystickController::PS4:
-          displayPS4Data();
-          break;
-        case JoystickController::PS3:
-          displayPS3Data();
-          break;
-        default:  
-        case JoystickController::XBOXONE:
-        case JoystickController::XBOX360:;
-          displayRawData();
-          break;
-      }
-      //for (uint8_t i = 0; i < 24; i++) {
-      //  Serial.printf(" %d:%d", i, psAxis[i]);
-      //}
-      //Serial.println();
-        
-     delay(100);
-     joystick1.joystickDataClear();
+    delay(100);
+    joystick1.joystickDataClear();
   }
 
   // See if we have some RAW data
   if (rawhid1) {
     int ch;
     uint8_t buffer[64];
-    uint8_t count_chars = 0; 
+    uint8_t count_chars = 0;
     memset(buffer, 0, sizeof(buffer));
     if (Serial.available()) {
       while (((ch = Serial.read()) != -1) && (count_chars < sizeof(buffer))) {
   uint8_t ltv;
   uint8_t rtv;
 
-    ltv = psAxis[3];
-    rtv = psAxis[4];
+  ltv = psAxis[3];
+  rtv = psAxis[4];
 
-    if ((ltv != joystick_left_trigger_value) || (rtv != joystick_right_trigger_value)) {
-      joystick_left_trigger_value = ltv;
-      joystick_right_trigger_value = rtv;
-      Serial.printf("Rumbling: %d, %d\r\n", ltv, rtv);
-      joystick1.setRumble(ltv, rtv);
-    }
+  if ((ltv != joystick_left_trigger_value) || (rtv != joystick_right_trigger_value)) {
+    joystick_left_trigger_value = ltv;
+    joystick_right_trigger_value = rtv;
+    Serial.printf("Rumbling: %d, %d\r\n", ltv, rtv);
+    joystick1.setRumble(ltv, rtv);
+  }
    
 
   if (buttons != buttons_prev) {
   if ((buttons & 0x02) && !(buttons_prev & 0x02)) show_raw_data = !show_raw_data;
   if ((buttons & 0x04) && !(buttons_prev & 0x04)) show_changed_data = !show_changed_data;
 
-  // See about maybe pair... 
+  // See about maybe pair...
   if ((buttons & 0x10000) && !(buttons_prev & 0x10000) && (buttons & 0x0001)) {
     // PS button just pressed and select button pressed act like PS4 share like...
     Serial.print("\nPS3 Pairing Request");
         Serial.println("  Pairing complete (I hope), make sure Bluetooth adapter is plugged in and try PS3 without USB");
       }
     }
-  } 
+  }
 
 
   if (show_raw_data)  {
     Serial.printf("L-Trig: %d, R-Trig: %d\r\n", psAxis[3], psAxis[4]);
     Serial.printf("Buttons: %x\r\n", buttons);
   }
-    uint8_t ltv;
-    uint8_t rtv;
+  uint8_t ltv;
+  uint8_t rtv;
 
-    ltv = psAxis[3];
-    rtv = psAxis[4];
+  ltv = psAxis[3];
+  rtv = psAxis[4];
 
-    if ((ltv != joystick_left_trigger_value) || (rtv != joystick_right_trigger_value)) {
-      joystick_left_trigger_value = ltv;
-      joystick_right_trigger_value = rtv;
-      Serial.printf("Rumbling: %d, %d\r\n", ltv, rtv);
-      joystick1.setRumble(ltv, rtv);
-    }
+  if ((ltv != joystick_left_trigger_value) || (rtv != joystick_right_trigger_value)) {
+    joystick_left_trigger_value = ltv;
+    joystick_right_trigger_value = rtv;
+    Serial.printf("Rumbling: %d, %d\r\n", ltv, rtv);
+    joystick1.setRumble(ltv, rtv);
+  }
+
+  if (buttons != buttons_prev) {
+    uint8_t leds = 0;
+    if (buttons & 0x8000) leds = 1;   //Srq
+    if (buttons & 0x2000) leds = 2;   //Cir
+    if (buttons & 0x1000) leds = 3;   //Tri
+    //Cross = 2
+    joystick1.setLEDs(leds);
+    buttons_prev = buttons;
+  }
+}
+
+void displayXBoxData()
+{
+  buttons = joystick1.getButtons();
+
+  // Use L3 (Left joystick button) to toggle Show Raw or not...
+  if ((buttons & 0x4000) && !(buttons_prev & 0x4000)) show_raw_data = !show_raw_data;
+  if ((buttons & 0x8000) && !(buttons_prev & 0x8000)) show_changed_data = !show_changed_data;
 
-    if (buttons != buttons_prev) {
-      uint8_t leds = 0;
-      if (buttons & 0x8000) leds = 1;   //Srq
-      if (buttons & 0x2000) leds = 2;   //Cir
-      if (buttons & 0x1000) leds = 3;   //Tri
-                                                 //Cross = 2
-      joystick1.setLEDs(leds);
-      buttons_prev = buttons;
+  if (show_raw_data)  {
+    displayRawData();
+  } else {
+    Serial.printf("LX: %d, LY: %d, RX: %d, RY: %d \r\n", psAxis[0], psAxis[1], psAxis[2], psAxis[5]);
+    Serial.printf("L-Trig: %d, R-Trig: %d\r\n", psAxis[3], psAxis[4]);
+    Serial.printf("Buttons: %x\r\n", buttons);
+  }
+  uint8_t ltv;
+  uint8_t rtv;
+
+  ltv = psAxis[3];
+  rtv = psAxis[4];
+
+  if ((ltv != joystick_left_trigger_value) || (rtv != joystick_right_trigger_value)) {
+    joystick_left_trigger_value = ltv;
+    joystick_right_trigger_value = rtv;
+    Serial.printf("Rumbling: %d, %d\r\n", ltv, rtv);
+    joystick1.setRumble(ltv, rtv);
+  }
+
+  if (buttons != buttons_prev) {
+    uint8_t leds = 0;
+    if (buttons & 0x8000) leds = 1;   //Srq
+    if (buttons & 0x2000) leds = 2;   //Cir
+    if (buttons & 0x1000) leds = 3;   //Tri
+    //Cross = 2
+    joystick1.setLEDs(leds);
+    buttons_prev = buttons;
   }
 }
- 
 
 void displayRawData() {
   uint64_t axis_mask = joystick1.axisMask();
   uint64_t changed_mask = joystick1.axisChangedMask();
-  if (!changed_mask) return;
+
+  buttons = joystick1.getButtons();
+
+  if (!changed_mask && (buttons == buttons_prev)) return;
 
   if (show_changed_data) {
     if (!changed_mask) return;
     changed_mask &= 0xfffffffffL; // try reducing which ones show...
     Serial.printf("%0x - ", joystick1.getButtons());
 
-    for (uint16_t index=0; changed_mask; index++) {
+    for (uint16_t index = 0; changed_mask; index++) {
       if (changed_mask & 1) {
         Serial.printf("%d:%02x ", index, psAxis[index]);
       }
     axis_mask &= 0xffffff;
     Serial.printf("%06x%06x: %06x - ", (uint32_t)(changed_mask >> 32), (uint32_t)(changed_mask & 0xffffffff), joystick1.getButtons());
 
-    for (uint16_t index=0; axis_mask; index++) {
+    for (uint16_t index = 0; axis_mask; index++) {
       Serial.printf("%02x ", psAxis[index]);
       axis_mask >>= 1;
     }
   }
- Serial.println();
- 
+  Serial.println();
+  buttons_prev = buttons;
+
 }
 
 
 bool OnReceiveHidData(uint32_t usage, const uint8_t *data, uint32_t len) {
   // Called for maybe both HIDS for rawhid basic test.  One is for the Teensy
-  // to output to Serial. while still having Raw Hid... 
+  // to output to Serial. while still having Raw Hid...
   if (usage == 0xffc90004) {
     // Lets trim off trailing null characters.
-    while ((len > 0) && (data[len-1] == 0)) {
+    while ((len > 0) && (data[len - 1] == 0)) {
       len--;
     }
     if (len) {
     Serial.print("RawHID data: ");
     Serial.println(usage, HEX);
     while (len) {
-      uint8_t cb = (len > 16)? 16 : len;
+      uint8_t cb = (len > 16) ? 16 : len;
       const uint8_t *p = data;
       uint8_t i;
       for (i = 0; i < cb; i++) {
       }
       Serial.print(": ");
       for (i = 0; i < cb; i++) {
-        Serial.write(((*data >= ' ')&&(*data <= '~'))? *data : '.');
+        Serial.write(((*data >= ' ') && (*data <= '~')) ? *data : '.');
         data++;
       }
       len -= cb;

joystick.cpp

 // Information came from several places on the web including: 
 // https://github.com/quantus/xbox-one-controller-protocol
 /************************************************************/
+// 20 00 C5 0E 00 00 00 00 00 00 F0 06 AD FB 7A 0A DD F7 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
+// 20 00 E0 0E 40 00 00 00 00 00 F0 06 AD FB 7A 0A DD F7 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
 typedef struct {
 	uint8_t type;
 	uint8_t const_0;
 	int16_t	axis[4];
 } xbox360data_t;
 
-static const uint8_t xbox_axis_order_mapping[] = {4, 5, 0, 1, 2, 3};
+static const uint8_t xbox_axis_order_mapping[] = {3, 4, 0, 1, 2, 5};
 
 void JoystickController::rx_data(const Transfer_t *transfer)
 {
-	print("JoystickController::rx_data: ");
+	print("JoystickController::rx_data (", joystickType_, DEC);
+	print("): ");
 	print_hexbytes((uint8_t*)transfer->buffer, transfer->length);
 
 	if (joystickType_ == XBOXONE) {
 			if (xb1d->buttons != buttons) {
 				buttons = xb1d->buttons;
 				anychange = true;
+				joystickEvent = true;
+				println("  Button Change: ", buttons, HEX);
 			}
 			for (uint8_t i = 0; i < sizeof (xbox_axis_order_mapping); i++) {
 				// The first two values were unsigned.