WIP - Start PS3 support

I have some PS3 support working... Hope I did not break PS4.... ;)

Also This assumes that you did a bind of the dongle to PS3 external, no support yet, I did this on RPI3...

Rumble needs work!

USBHost_t36.h

 	virtual bool process_bluetooth_HID_data(const uint8_t *data, uint16_t length) {return false;}
 	virtual void release_bluetooth() {};
 	virtual void remoteNameComplete(const uint8_t *remoteName) {};
+	virtual void connectionComplete(void) {};
 	void add_to_list();
 	BTHIDInput *next = NULL;
 	friend class BluetoothController;
 protected:
-	uint8_t  needs_connect_to_device = false;
+	enum {SP_NEED_CONNECT=0x1, SP_PS3_IDS=0x2};
+	uint8_t  special_process_required = 0;
 	Device_t *btdevice = NULL;
 
 };
 	virtual bool process_bluetooth_HID_data(const uint8_t *data, uint16_t length);
 	virtual void release_bluetooth();
 	virtual void remoteNameComplete(const uint8_t *remoteName);
+	virtual void connectionComplete(void);
 
 private:
 
 
 class BluetoothController: public USBDriver {
 public:
-	BluetoothController(USBHost &host, bool pair = false, const char *pin = "0000") : do_pair_device_(pair), pair_pincode_(pin) 
+	BluetoothController(USBHost &host, bool pair = false, const char *pin = "0000") : do_pair_device_(pair), pair_pincode_(pin), delayTimer_(this) 
 			 { init(); }
 
 	enum {MAX_ENDPOINTS=4, NUM_SERVICES=4, };  // Max number of Bluetooth services - if you need more than 4 simply increase this number
     const uint8_t* 	myBDAddr(void) {return my_bdaddr_;}
 
 	// BUGBUG version to allow some of the controlled objects to call?
-
-    void sendL2CapCommand(uint8_t* data, uint8_t nbytes, uint8_t channelLow = 0x01, uint8_t channelHigh = 0x00) {
-    	USBHDBGSerial.printf("sendL2CapCommand: %x %d %x %x : control: %x\n", (uint32_t)data, nbytes, channelLow, channelHigh, control_scid_);
-    	sendL2CapCommand (device_connection_handle_, data, nbytes, channelLow, channelHigh);
-      }
+    enum {CONTROL_SCID=-1};
+    void sendL2CapCommand(uint8_t* data, uint8_t nbytes, int channel = (int)0x0001);
 
 protected:
 	virtual bool claim(Device_t *device, int type, const uint8_t *descriptors, uint32_t len);
 	virtual void control(const Transfer_t *transfer);
 	virtual void disconnect();
-	//virtual void timer_event(USBDriverTimer *whichTimer);
+	virtual void timer_event(USBDriverTimer *whichTimer);
 
 	BTHIDInput * find_driver(uint32_t device_type);
+
+	// Hack to allow PS3 to maybe change values
+    uint16_t		connection_rxid_ = 0;
+    uint16_t		control_dcid_ = 0x70;
+    uint16_t		interrupt_dcid_ = 0x71;
+    uint16_t		interrupt_scid_;
+    uint16_t		control_scid_;
+
+
 private:
 	friend class BTHIDInput;
 	static void rx_callback(const Transfer_t *transfer);
 
 	bool 			do_pair_device_;	// Should we do a pair for a new device?
 	const char		*pair_pincode_;	// What pin code to use for the pairing
+	USBDriverTimer 	delayTimer_;
     uint8_t 		my_bdaddr_[6];	// The bluetooth dongles Bluetooth address.
     uint8_t			features[8];	// remember our local features.
     BTHIDInput * 	device_driver_ = nullptr;;
     uint8_t			device_clock_offset_[2];
     uint32_t		device_class_;	// class of device. 
     uint16_t		device_connection_handle_;	// handle to connection 
-    uint16_t		connection_rxid_ = 0;
-    uint16_t		control_dcid_ = 0x70;
-    uint16_t		interrupt_dcid_ = 0x71;
-    uint16_t		interrupt_scid_;
-    uint16_t		control_scid_;
 	uint8_t    		remote_ver_;
 	uint16_t		remote_man_;
 	uint8_t			remote_subv_;
+	uint8_t			connection_complete_ = false;	//
 
 	typedef struct {
 		uint16_t 	idVendor;

bluetooth.cpp

 #define print   USBHost::print_
 #define println USBHost::println_//#define DEBUG_BT
 
-#define DEBUG_BT
-#define DEBUG_BT_VERBOSE
+//#define DEBUG_BT
+//#define DEBUG_BT_VERBOSE
 
 #ifndef DEBUG_BT
 #undef DEBUG_BT_VERBOSE
-void DBGPrintf(...) {};
+void inline DBGPrintf(...) {};
 #else
 #define DBGPrintf USBHDBGSerial.printf
 #endif
 
 #ifndef DEBUG_BT_VERBOSE
-void VDBGPrintf(...) {};
+void inline VDBGPrintf(...) {};
 #else
 #define VDBGPrintf USBHDBGSerial.printf
 #endif
 		device_driver_->release_bluetooth();
 		device_driver_ = nullptr;
 	}
+	connection_complete_ = false;
 }
 
+void BluetoothController::timer_event(USBDriverTimer *whichTimer)
+{
+	
+}
 
 
 void BluetoothController::control(const Transfer_t *transfer)
 			break;
 		case HCI_WRITE_SCAN_ENABLE:				//0x0c1a
 			DBGPrintf("Write_Scan_enable Completed\n");
-			if (device_connection_handle_) {
-				// Lets see if we can get the remote information
-				//sendHCIRemoteVersionInfoRequest();
+
+			if (device_driver_ && connection_complete_) {	// We have a driver call their 
+				device_driver_->connectionComplete();
+				connection_complete_ = false;	// only call once
 			}
 			break;
 		case HCI_WRITE_SSP_MODE:					//0x0c56
 	if (do_pair_device_) {
 		sendHCIAuthenticationRequested();
 		pending_control_ = PC_AUTHENTICATION_REQUESTED;
-	} else if (device_driver_ && device_driver_->needs_connect_to_device) {
+	} else if (device_driver_ && (device_driver_->special_process_required & BTHIDInput::SP_NEED_CONNECT)) {
 		DBGPrintf("   Needs connect to device(PS4?)\n");
 		// The PS4 requires a connection request to it. 
 		delay(1);
 	    packet[0] = 0x43; 
 	    packet[1] = 0x02;      // Report ID
 	    USBHDBGSerial.printf("SixAxis Command Issued!\r\n");
-     	sendL2CapCommand(packet, sizeof(packet), 0x40, 0x00);	
+     	sendL2CapCommand(packet, sizeof(packet), 0x40);	
 	}
 }
 
 	if (device_driver_) {
 		device_driver_->release_bluetooth();
 		device_driver_ = nullptr;
+    	
+    	// Restore to normal... 
+    	control_dcid_ = 0x70;
+    	interrupt_dcid_ = 0x71;
 	}
 	// Probably should clear out connection data. 
 	device_connection_handle_ = 0;
 		DBGPrintf("\n");
 	}
 	if (device_driver_) {
-	/*
-		if (!device_driver_->btstrbuf) {
-			device_driver_->btstrbuf = USBHost::allocate_string_buffer();
-			if (device_driver_->btstrbuf) {
-
-			}
-		}
-	*/
 		device_driver_->remoteNameComplete(&rxbuf_[9]);
+		if (device_driver_->special_process_required & BTHIDInput::SP_PS3_IDS) {
+			// Real hack see if PS3... 
+	    	control_dcid_ = 0x40;
+	    	interrupt_dcid_ = 0x41;
+	    }
 	}
 
 	// Lets now try to accept the connection. 
 /************************************************************/
 /*                    L2CAP Commands                        */
 
+// Public wrrapper function
+void BluetoothController::sendL2CapCommand(uint8_t* data, uint8_t nbytes, int channel) {
+	uint16_t channel_out;
+	switch (channel) {
+		case CONTROL_SCID:
+			channel_out = control_scid_;
+			break;
+		default:
+			channel_out = (uint16_t)channel;
+	}
+	DBGPrintf("sendL2CapCommand: %x %d %x\n", (uint32_t)data, nbytes, channel, channel_out);
+	sendL2CapCommand (device_connection_handle_, data, nbytes, channel_out & 0xff, (channel_out >> 8) & 0xff);
+}
+
+
 void BluetoothController::sendL2CapCommand(uint16_t handle, uint8_t* data, uint8_t nbytes, uint8_t channelLow, uint8_t channelHigh)
 {
     txbuf_[0] = handle & 0xff; // HCI handle with PB,BC flag
 		data[8]+((uint16_t)data[9] << 8), data[10]+((uint16_t)data[11] << 8));
 	if (scid == control_dcid_) {
 		// Set HID Boot mode
-		setHIDProtocol(HID_BOOT_PROTOCOL);  //
+		// Don't do if PS3...
+		if (!(device_driver_->special_process_required & BTHIDInput::SP_PS3_IDS)) {
+			setHIDProtocol(HID_BOOT_PROTOCOL);  //
+		}
 		//setHIDProtocol(HID_RPT_PROTOCOL);  //HID_RPT_PROTOCOL
 		if (do_pair_device_) {
 			pending_control_tx_ = STATE_TX_SEND_CONNECT_INT;
-		} else if (device_driver_ && device_driver_->needs_connect_to_device) {
+		} else if (device_driver_ && (device_driver_->special_process_required & BTHIDInput::SP_NEED_CONNECT)) {
 			DBGPrintf("   Needs connect to device INT(PS4?)\n");
 			// The PS4 requires a connection request to it. 
 			pending_control_tx_ = STATE_TX_SEND_CONNECT_INT;
 	} else if (scid == interrupt_dcid_) {
 		// Enable SCan to page mode
 		sendHCIWriteScanEnable(2);
+		connection_complete_ = true;
 	}
 }
 

examples/JoystickBT/JoystickBT.ino

 //BluetoothController bluet(myusb, true, "0000");   // Version does pairing to device
 BluetoothController bluet(myusb);   // version assumes it already was paired
 int user_axis[64];
-uint8_t buttons_prev = 0;
-uint8_t button;
+uint16_t buttons_prev = 0;
+uint16_t buttons;
 RawHIDController rawhid1(myusb);
 RawHIDController rawhid2(myusb, 0xffc90004);
 
 
 bool hid_driver_active[CNT_DEVICES] = {false, false, false};
 bool show_changed_only = false; 
+bool show_raw_data = false;
 
 uint8_t joystick_left_trigger_value = 0;
 uint8_t joystick_right_trigger_value = 0;
   Serial4.println("\n" __FILE__ " " __DATE__ " " __TIME__);
   Serial4.println("\n********\n T4 connected Serial4 *******\n");
 */
-Serial1.begin(1843200);
+Serial1.begin(2000000);
   while (!Serial) ; // wait for Arduino Serial Monitor
   //debTraceShow_tt( -2, "", "", "" );
   //Serial4.println("\n" __FILE__ " " __DATE__ " " __TIME__);
       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:;
+          displayRawData();
+          break;
+      }
       //for (uint8_t i = 0; i < 24; i++) {
       //  Serial.printf(" %d:%d", i, psAxis[i]);
       //}
       //Serial.println();
         
-      Serial.printf("LX: %d, LY: %d, RX: %d, RY: %d \r\n", psAxis[1], psAxis[2], psAxis[3], psAxis[4]);
-      Serial.printf("L-Trig: %d, R-Trig: %d, Trig-Button: %d \r\n", psAxis[8], psAxis[9], psAxis[6]);
-      Serial.printf("Buttons: %d, PS: %d\r\n", psAxis[5], psAxis[7]);
-      Serial.printf("Arrows: %d\r\n", psAxis[0]);
-      Serial.printf("Battery level percentage: %2f.0 \r\n", (((float) psAxis[12])/255.0f)*100.0f);
-     
-      Serial.println();
-
-    uint8_t ltv;
-    uint8_t rtv;
-
-        ltv = psAxis[8];
-        rtv = psAxis[9];
-
-        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);
-        }
-        
-      /* Arrow Buttons (psAxis[0]):
-       *    0x08 is released, 
-       *    0=N, 1=NE, 2=E, 3=SE, 4=S, 
-       *    5=SW, 6=W, 7=NW)
-       */
-
-      if (psAxis[5] != buttons_prev) {
-          uint8_t lr = (psAxis[5] & 1) ? 0xff : 0;   //Srq
-          uint8_t lg = (psAxis[5] & 4) ? 0xff : 0;   //Cir
-          uint8_t lb = (psAxis[5] & 8) ? 0xff : 0;   //Tri
-                                                     //Cross = 2
-          Serial.print(psAxis[5]); Serial.print(", ");
-          Serial.print(lr); Serial.print(", "); 
-          Serial.print(lg); Serial.print(", "); 
-          Serial.println(lb); 
-
-          joystick1.setLEDs(lr, lg, lb);
-      }
-          buttons_prev =psAxis[5];
 
      joystick1.joystickDataClear();
   }
   }
 }
 
+void displayPS4Data()
+{
+  Serial.printf("LX: %d, LY: %d, RX: %d, RY: %d \r\n", psAxis[1], psAxis[2], psAxis[3], psAxis[4]);
+  Serial.printf("L-Trig: %d, R-Trig: %d, Trig-Button: %d \r\n", psAxis[8], psAxis[9], psAxis[6]);
+  Serial.printf("Buttons: %d, PS: %d\r\n", psAxis[5], psAxis[7]);
+  Serial.printf("Arrows: %d\r\n", psAxis[0]);
+  Serial.printf("Battery level percentage: %2f.0 \r\n", (((float) psAxis[12])/255.0f)*100.0f);
+
+  Serial.println();
+
+  uint8_t ltv;
+  uint8_t rtv;
+
+    ltv = psAxis[8];
+    rtv = psAxis[9];
+
+    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);
+    }
+    
+  /* Arrow Buttons (psAxis[0]):
+   *    0x08 is released, 
+   *    0=N, 1=NE, 2=E, 3=SE, 4=S, 
+   *    5=SW, 6=W, 7=NW)
+   */
+
+  if (psAxis[5] != buttons_prev) {
+      uint8_t lr = (psAxis[5] & 1) ? 0xff : 0;   //Srq
+      uint8_t lg = (psAxis[5] & 4) ? 0xff : 0;   //Cir
+      uint8_t lb = (psAxis[5] & 8) ? 0xff : 0;   //Tri
+                                                 //Cross = 2
+      Serial.print(psAxis[5]); Serial.print(", ");
+      Serial.print(lr); Serial.print(", "); 
+      Serial.print(lg); Serial.print(", "); 
+      Serial.println(lb); 
+
+      joystick1.setLEDs(lr, lg, lb);
+      buttons_prev =psAxis[5];
+  }
+}
+
+void displayPS3Data()
+
+{
+  buttons = psAxis[2] | ((uint16_t)psAxis[3] << 8);
+
+  // Use L3 (Left joystick button) to toggle Show Raw or not...
+  if ((buttons & 0x02) && !(buttons_prev & 0x02)) show_raw_data = !show_raw_data;
+  if (show_raw_data)  {
+    displayRawData();
+  } else {
+    Serial.printf("LX: %d, LY: %d, RX: %d, RY: %d \r\n", psAxis[6], psAxis[7], psAxis[8], psAxis[9]);
+    Serial.printf("L-Trig: %d, R-Trig: %d\r\n", psAxis[18], psAxis[19]);
+    Serial.printf("Buttons: %x\r\n", buttons);
+  }
+    uint8_t ltv;
+    uint8_t rtv;
+
+    ltv = psAxis[18];
+    rtv = psAxis[19];
+
+    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;
+
+  Serial.printf("%lx %lx:", axis_mask, changed_mask);
+  for (uint16_t index=0; axis_mask; index++) {
+    Serial.printf("%02x ", psAxis[index]);
+    axis_mask >>= 1;
+  }
+  Serial.println();
+  //for (uint8_t i = 0; i < 24; i++) {
+  //  Serial.printf(" %d:%d", i, psAxis[i]);
+  //}
+  //Serial.println();
+ 
+}
+
+
 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... 

joystick.cpp

 #define print   USBHost::print_
 #define println USBHost::println_
 
+#define DEBUG_JOYSTICK
+#ifdef  DEBUG_JOYSTICK
+#define DBGPrintf USBHDBGSerial.printf
+#else
+#define DBGPrintf(...) 
+#endif
+
 // PID/VID to joystick mapping - Only the XBOXOne is used to claim the USB interface directly, 
 // The others are used after claim-hid code to know which one we have and to use it for 
 // doing other features.  
 	    packet[7] = leds_[1]; 
 	    packet[8] = leds_[2];
 	    // 9, 10 flash ON, OFF times in 100ths of second?  2.5 seconds = 255
-	    USBHDBGSerial.printf("Joystick update Rumble/LEDs\n");
+	    DBGPrintf("Joystick update Rumble/LEDs\n");
 		return driver_->sendPacket(packet, 32);
 	} else if (btdriver_) {
 		uint8_t packet[79];
 	    packet[11] = leds_[2];
 
 	    // 12, 13 flash ON, OFF times in 100ths of sedond?  2.5 seconds = 255
-	    USBHDBGSerial.printf("Joystick update Rumble/LEDs\n");
-     	btdriver_->sendL2CapCommand(packet, sizeof(packet), 0x40, 0x00);
+	    DBGPrintf("Joystick update Rumble/LEDs\n");
+     	btdriver_->sendL2CapCommand(packet, sizeof(packet), 0x40);
 
      	return true;
 	}
         0x00, 0x00, 0x00 };
 
 bool JoystickController::transmitPS3UserFeedbackMsg() {
-if (!driver_) return false;
-    memcpy(txbuf_, PS3_USER_FEEDBACK_INIT, 48);
-
-    txbuf_[1] = rumble_lValue_? rumble_timeout_ : 0;
-    txbuf_[2] = rumble_lValue_; // Small Rumble
-    txbuf_[3] = rumble_rValue_? rumble_timeout_ : 0; 
-    txbuf_[4] = rumble_rValue_; // Big rumble
-    txbuf_[9] = leds_[0] << 1; // RGB value 
-    //USBHDBGSerial.printf("\nJoystick update Rumble/LEDs %d %d %d %d %d\n",  txbuf_[1], txbuf_[2],  txbuf_[3],  txbuf_[4],  txbuf_[9]);
-	return driver_->sendControlPacket(0x21, 9, 0x201, 0, 48, txbuf_); 
+	if (driver_) {
+	    memcpy(txbuf_, PS3_USER_FEEDBACK_INIT, 48);
+
+	    txbuf_[1] = rumble_lValue_? rumble_timeout_ : 0;
+	    txbuf_[2] = rumble_lValue_; // Small Rumble
+	    txbuf_[3] = rumble_rValue_? rumble_timeout_ : 0; 
+	    txbuf_[4] = rumble_rValue_; // Big rumble
+	    txbuf_[9] = leds_[0] << 1; // RGB value 
+	    //DBGPrintf("\nJoystick update Rumble/LEDs %d %d %d %d %d\n",  txbuf_[1], txbuf_[2],  txbuf_[3],  txbuf_[4],  txbuf_[9]);
+		return driver_->sendControlPacket(0x21, 9, 0x201, 0, 48, txbuf_); 
+	} else if (btdriver_) {
+		txbuf_[0] = 0x52;
+		txbuf_[1] = 0x1;
+	    memcpy(&txbuf_[2], PS3_USER_FEEDBACK_INIT, 48);
+
+	    txbuf_[3] = rumble_lValue_? rumble_timeout_ : 0;
+	    txbuf_[4] = rumble_lValue_; // Small Rumble
+	    txbuf_[5] = rumble_rValue_? rumble_timeout_ : 0; 
+	    txbuf_[6] = rumble_rValue_; // Big rumble
+	    txbuf_[11] = leds_[0] << 1; // RGB value 
+	    DBGPrintf("\nJoystick update Rumble/LEDs %d %d %d %d %d\n",  txbuf_[3], txbuf_[4],  txbuf_[5],  txbuf_[6],  txbuf_[11]);
+     	btdriver_->sendL2CapCommand(txbuf_, 50, BluetoothController::CONTROL_SCID);
+     	return true;
+	}
+	return false;
 }
 
 //*****************************************************************************
 			additional_axis_usage_count_ = 0;
 			axis_change_notify_mask_ = 0x3ff;	// Start off assume only the 10 bits...
 	}
-	USBHDBGSerial.printf("Claim Additional axis: %x %x %d\n", additional_axis_usage_page_, additional_axis_usage_start_, additional_axis_usage_count_);
+	DBGPrintf("Claim Additional axis: %x %x %d\n", additional_axis_usage_page_, additional_axis_usage_start_, additional_axis_usage_count_);
 	return CLAIM_REPORT;
 }
 
 
 void JoystickController::hid_input_data(uint32_t usage, int32_t value)
 {
-	//USBHDBGSerial.printf("Joystick: usage=%X, value=%d\n", usage, value);
+	//DBGPrintf("Joystick: usage=%X, value=%d\n", usage, value);
 	uint32_t usage_page = usage >> 16;
 	usage &= 0xFFFF;
 	if (usage_page == 9 && usage >= 1 && usage <= 32) {
 		}
 	} else if (usage_page == additional_axis_usage_page_) {
 		// see if the usage is witin range.
-		//USBHDBGSerial.printf("UP: usage_page=%x usage=%x User: %x %d\n", usage_page, usage, user_buttons_usage_start, user_buttons_count_);
+		//DBGPrintf("UP: usage_page=%x usage=%x User: %x %d\n", usage_page, usage, user_buttons_usage_start, user_buttons_count_);
 		if ((usage >= additional_axis_usage_start_) && (usage < (additional_axis_usage_start_ + additional_axis_usage_count_))) {
 			// We are in the user range. 
 			uint16_t usage_index = usage - additional_axis_usage_start_ + STANDARD_AXIS_COUNT;
 				}
 				axis_mask_ |= ((uint64_t)1 << usage_index);		// Keep record of which axis we have data on.
 			}
-			//USBHDBGSerial.printf("UB: index=%x value=%x\n", usage_index, value);
+			//DBGPrintf("UB: index=%x value=%x\n", usage_index, value);
 		}
 
 	} else {
-		USBHDBGSerial.printf("UP: usage_page=%x usage=%x add: %x %x %d\n", usage_page, usage, additional_axis_usage_page_, additional_axis_usage_start_, additional_axis_usage_count_);
+		DBGPrintf("UP: usage_page=%x usage=%x add: %x %x %d\n", usage_page, usage, additional_axis_usage_page_, additional_axis_usage_start_, additional_axis_usage_count_);
 
 	}
 	// TODO: hat switch?
 
 bool JoystickController::hid_process_out_data(const Transfer_t *transfer) 
 {
-	//USBHDBGSerial.printf("JoystickController::hid_process_out_data\n");
+	//DBGPrintf("JoystickController::hid_process_out_data\n");
 	return true;
 }
 
 	            println("XBox360w - controllerStatus: ", xb360d->controller_status, HEX);
         } else if(xb360d->id_or_type == 0x01) { // Lets only process report 1.
 			//const uint8_t *pbuffer = (uint8_t*)transfer->buffer;
-        	//for (uint8_t i = 0; i < transfer->length; i++) USBHDBGSerial.printf("%02x ", pbuffer[i]);
-        	//USBHDBGSerial.printf("\n");
+        	//for (uint8_t i = 0; i < transfer->length; i++) DBGPrintf("%02x ", pbuffer[i]);
+        	//DBGPrintf("\n");
 	        
 	        if (buttons != xb360d->buttons) {
 	        	buttons = xb360d->buttons;
 bool JoystickController::claim_bluetooth(BluetoothController *driver, uint32_t bluetooth_class) 
 {
 	if ((((bluetooth_class & 0xff00) == 0x2500) || (((bluetooth_class & 0xff00) == 0x500))) && ((bluetooth_class & 0x3C) == 0x08)) {
-		USBHDBGSerial.printf("JoystickController::claim_bluetooth TRUE\n");
+		DBGPrintf("JoystickController::claim_bluetooth TRUE\n");
 		btdriver_ = driver;
 		btdevice = (Device_t*)driver;	// remember this way 
 		return true;
 	// Example data from PS4 controller
 	//01 7e 7f 82 84 08 00 00 00 00
 	//   LX LY RX RY BT BT PS LT RT
-	//USBHDBGSerial.printf("JoystickController::process_bluetooth_HID_data\n");
+	//DBGPrintf("JoystickController::process_bluetooth_HID_data\n");
 	// May have to look at this one with other controllers...
 	if (data[0] == 1) {
 		//print("  Joystick Data: ");
 		//print_hexbytes(data, length);
-		USBHDBGSerial.printf("  Joystick Data: ");
+//		DBGPrintf("  Joystick Data: ");
 		uint64_t mask = 0x1;
 		axis_mask_ = 0;
 		axis_changed_mask_ = 0;
+
+		if (length > TOTAL_AXIS_COUNT) length = TOTAL_AXIS_COUNT;	// don't overflow arrays...
 		for (uint16_t i = 0; i < length; i++ ) {
 			axis_mask_ |= mask;
 			if(data[i] != axis[i]) { 
 				axis[i] = data[i];
 			} 
 			mask <<= 1;	// shift down the mask.
-			USBHDBGSerial.printf("%02x ", axis[i]);
+//			DBGPrintf("%02x ", axis[i]);
 		}
-		USBHDBGSerial.printf("\n");
+//		DBGPrintf("\n");
 		joystickEvent = true;
 		connected_ = true;
 		return true;
 	} else if(data[0] == 0x11){
-		USBHDBGSerial.printf("  Joystick Data: ");
+		DBGPrintf("  Joystick Data: ");
 		uint64_t mask = 0x1;
 		axis_mask_ = 0;
 		axis_changed_mask_ = 0;
 		//set arrow buttons to axis[0]
 		tmp_data[0] = tmp_data[5] & ((1 << 4) - 1);
 		//set buttons for last 4bits in the axis[5]
-		tmp_data[5] = tmp_data[5] >> 4;		
+		tmp_data[5] = tmp_data[5] >> 4;
+
 		for (uint16_t i = 0; i < (length-2); i++ ) {
 			if(tmp_data[i] != axis[i]) { 
 				axis_changed_mask_ |= mask;
 				axis[i] = tmp_data[i];
 			} 
 			mask <<= 1;	// shift down the mask.
-			USBHDBGSerial.printf("%02x ", axis[i]);
+			DBGPrintf("%02x ", axis[i]);
 		}
-		USBHDBGSerial.printf("\n");
+		DBGPrintf("\n");
 		joystickEvent = true;
 		connected_ = true;
 	}
 	if (!remoteName) return;
 
 	if (strncmp((const char *)remoteName, "Wireless Controller", 19) == 0) {
-		USBHDBGSerial.printf("  JoystickController::remoteNameComplete %s - set to PS4\n", remoteName);
-		needs_connect_to_device = true; 		// We need to force this. 
-		joystickType = PS4;	
+		DBGPrintf("  JoystickController::remoteNameComplete %s - set to PS4\n", remoteName);
+		special_process_required = SP_NEED_CONNECT; 		// We need to force this. 
+		joystickType = PS4;
+	} else if (strncmp((const char *)remoteName, "PLAYSTATION(R)3 Controller", 26) == 0) {
+		DBGPrintf("  JoystickController::remoteNameComplete %s - set to PS3\n", remoteName);
+		special_process_required = SP_PS3_IDS; 		// PS3 maybe needs different IDS. 
+		joystickType = PS3;
+
+		// Hack to see if setting control/interrupt ids will help on PS3...
+//    	btdriver_->control_dcid_ = 0x40;
+ //   	btdriver_->interrupt_dcid_ = 0x41;
+
+	}
+
+}
+
+void JoystickController::connectionComplete() 
+{
+	DBGPrintf("  JoystickController::connectionComplete joystick type %d\n", joystickType);
+	if (joystickType == PS4) {
+		uint8_t packet[2];
+		packet[0] = 0x43; // HID BT Get_report (0x40) | Report Type (Feature 0x03)
+		packet[1] = 0x02; // Report ID
+		DBGPrintf("Set PS4 report\n");
+		delay(1);
+		btdriver_->sendL2CapCommand(packet, sizeof(packet), 0x40);
+
+	} else if (joystickType == PS3) {
+	uint8_t packet[6];
+		packet[0] = 0x53; // HID BT Set_report (0x50) | Report Type (Feature 0x03)
+		packet[1] = 0xF4; // Report ID
+		packet[2] = 0x42; // Special PS3 Controller enable commands
+		packet[3] = 0x03;
+		packet[4] = 0x00;
+		packet[5] = 0x00;
+
+		DBGPrintf("enable six axis\n");
+		delay(1);
+		btdriver_->sendL2CapCommand(packet, sizeof(packet), BluetoothController::CONTROL_SCID);
 	}
 
 }
 	btdevice = nullptr;	// remember this way 
 	btdriver_ = nullptr;
 	connected_ = false;
-	needs_connect_to_device = false;
+	special_process_required = false;
 
 }