Merge pull request #14 from KurtE/PS3_Motion_WIP2-Bluetooth

Ps3 motion wip2 bluetooth

USBHost_t36.h

 
 
 //#define USBHOST_PRINT_DEBUG
-#define USBHDBGSerial	Serial1
+//#define USBHDBGSerial	Serial1
 
 
 #ifndef USBHDBGSerial
 
 	// set functions functionality depends on underlying joystick. 
     bool setRumble(uint8_t lValue, uint8_t rValue, uint8_t timeout=0xff);
-    // setLEDs on PS4(RGB), PS3 simple LED setting (only uses lr)
-    bool setLEDs(uint8_t lr, uint8_t lg=0, uint8_t lb=0);  // sets Leds, 
+    // setLEDs on PS4(RGB), PS3 simple LED setting (only uses lb)
+    bool setLEDs(uint8_t lr, uint8_t lg, uint8_t lb);  // sets Leds, 
+    bool inline setLEDs(uint32_t leds) {return setLEDs((leds >> 16) & 0xff, (leds >> 8) & 0xff, leds & 0xff);}  // sets Leds - passing one arg for all leds 
 	enum { STANDARD_AXIS_COUNT = 10, ADDITIONAL_AXIS_COUNT = 54, TOTAL_AXIS_COUNT = (STANDARD_AXIS_COUNT+ADDITIONAL_AXIS_COUNT) };
-	typedef enum { UNKNOWN=0, PS3, PS4, XBOXONE, XBOX360} joytype_t;
+	typedef enum { UNKNOWN=0, PS3, PS4, XBOXONE, XBOX360, PS3_MOTION} joytype_t;
 	joytype_t joystickType() {return joystickType_;} 
 
 	// PS3 pair function. hack, requires that it be connect4ed by USB and we have the address of the Bluetooth dongle...
 	joytype_t mapVIDPIDtoJoystickType(uint16_t idVendor, uint16_t idProduct, bool exclude_hid_devices);
 	bool transmitPS4UserFeedbackMsg();
 	bool transmitPS3UserFeedbackMsg();
+	bool transmitPS3MotionUserFeedbackMsg();
 	bool mapNameToJoystickType(const uint8_t *remoteName);
 
 	bool anychange = false;
     const uint8_t* 	myBDAddr(void) {return my_bdaddr_;}
 
 	// BUGBUG version to allow some of the controlled objects to call?
-    enum {CONTROL_SCID=-1};
+    enum {CONTROL_SCID=-1, INTERRUPT_SCID=-2};
     void sendL2CapCommand(uint8_t* data, uint8_t nbytes, int channel = (int)0x0001);
 
 protected:

bluetooth.cpp

 #define println USBHost::println_//#define DEBUG_BT
 
 //#define DEBUG_BT
-#define DEBUG_BT_VERBOSE
+//#define DEBUG_BT_VERBOSE
 
 #ifndef DEBUG_BT
 #undef DEBUG_BT_VERBOSE
 		case CONTROL_SCID:
 			channel_out = control_scid_;
 			break;
+		case INTERRUPT_SCID:
+			channel_out = interrupt_scid_;
+			break;
 		default:
 			channel_out = (uint16_t)channel;
 	}

ehci.cpp

 	transfer = allocate_Transfer();
 	if (!transfer) return false;
 	data = transfer;
-	for (count=(len >> 14); count; count--) {
-		next = allocate_Transfer();
-		if (!next) {
-			// free already-allocated qTDs
-			while (1) {
-				next = (Transfer_t *)transfer->qtd.next;
-				free_Transfer(transfer);
-				if (transfer == data) break;
-				transfer = next;
+	if (len) {
+		for (count=((len-1) >> 14); count; count--) {
+			next = allocate_Transfer();
+			if (!next) {
+				// free already-allocated qTDs
+				while (1) {
+					next = (Transfer_t *)transfer->qtd.next;
+					free_Transfer(transfer);
+					if (transfer == data) break;
+					transfer = next;
+				}
+				return false;
 			}
-			return false;
+			data->qtd.next = (uint32_t)next;
+			data = next;
 		}
-		data->qtd.next = (uint32_t)next;
-		data = next;
-	}
+	}	
 	// last qTD needs info for followup
 	data->qtd.next = 1;
 	data->pipe = pipe;

examples/Bluetooth/JoystickBT/JoystickBT.ino

 const char * hid_driver_names[CNT_DEVICES] = {"Joystick1", "RawHid1", "RawHid2"};
 
 bool hid_driver_active[CNT_DEVICES] = {false, false, false};
+
+BTHIDInput *bthiddrivers[] = {&joystick1};
+#define CNT_BTHIDDEVICES (sizeof(bthiddrivers)/sizeof(bthiddrivers[0]))
+const char * bthid_driver_names[CNT_HIDDEVICES] = {"joystick"};
+bool bthid_driver_active[CNT_HIDDEVICES] = {false};
+
+
 bool show_changed_only = false;
 bool show_raw_data = false;
 bool show_changed_data = false;
 bool first_joystick_message = true;
 uint8_t last_bdaddr[6] = {0, 0, 0, 0, 0, 0};
 
+// ps3 motion on USB does not do much, but see if we can pair it and maybe change
+// color of bulb... 
+uint32_t PS3_MOTION_timer = 0;
+uint8_t  PS3_MOTION_tried_to_pair_state = 0;
+#define PS3_MOTION_PERIOD 2500 // not sure yet what would be good period for this..
+
+//=============================================================================
+// Setup
+//=============================================================================
 void setup()
 {
 
   rawhid2.attachReceive(OnReceiveHidData);
 }
 
-
+//=============================================================================
+// Loop
+//=============================================================================
 void loop()
 {
   myusb.Task();
       }
     }
   }
-
-  for (uint8_t i = 0; i < CNT_DEVICES; i++) {
-    if (*drivers[i] != driver_active[i]) {
-      if (driver_active[i]) {
-        Serial.printf("*** Device %s - disconnected ***\n", driver_names[i]);
-        driver_active[i] = false;
-      } else {
-        Serial.printf("*** Device %s %x:%x - connected ***\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());
-        driver_active[i] = true;
-
-        const uint8_t *psz = drivers[i]->manufacturer();
-        if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
-        psz = drivers[i]->product();
-        if (psz && *psz) Serial.printf("  product: %s\n", psz);
-        psz = drivers[i]->serialNumber();
-        if (psz && *psz) Serial.printf("  Serial: %s\n", psz);
-
-        if (drivers[i] == &bluet) {
-          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 < CNT_HIDDEVICES; i++) {
-    if (*hiddrivers[i] != hid_driver_active[i]) {
-      if (hid_driver_active[i]) {
-        Serial.printf("*** HID Device %s - disconnected ***\n", hid_driver_names[i]);
-        hid_driver_active[i] = false;
-      } else {
-        Serial.printf("*** HID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());
-        hid_driver_active[i] = true;
-
-        const uint8_t *psz = hiddrivers[i]->manufacturer();
-        if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
-        psz = hiddrivers[i]->product();
-        if (psz && *psz) Serial.printf("  product: %s\n", psz);
-        psz = hiddrivers[i]->serialNumber();
-        if (psz && *psz) Serial.printf("  Serial: %s\n", psz);
-      }
-    }
-  }
+  // check to see if the device list has changed:
+  UpdateActiveDeviceInfo();
+  processPS3MotionTimer();
 
   if (joystick1.available()) {
     if (first_joystick_message) {
       case JoystickController::PS3:
         displayPS3Data();
         break;
+      case JoystickController::PS3_MOTION:
+        displayPS3MotionData();
+        break;
       case JoystickController::XBOXONE:
       case JoystickController::XBOX360:
         displayXBoxData();
   }
 }
 
+//=============================================================================
+// UpdateActiveDeviceInfo
+//=============================================================================
+void UpdateActiveDeviceInfo() {
+  for (uint8_t i = 0; i < CNT_DEVICES; i++) {
+    if (*drivers[i] != driver_active[i]) {
+      if (driver_active[i]) {
+        Serial.printf("*** Device %s - disconnected ***\n", driver_names[i]);
+        driver_active[i] = false;
+      } else {
+        Serial.printf("*** Device %s %x:%x - connected ***\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());
+        driver_active[i] = true;
+
+        const uint8_t *psz = drivers[i]->manufacturer();
+        if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
+        psz = drivers[i]->product();
+        if (psz && *psz) Serial.printf("  product: %s\n", psz);
+        psz = drivers[i]->serialNumber();
+        if (psz && *psz) Serial.printf("  Serial: %s\n", psz);
+
+        if (drivers[i] == &bluet) {
+          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 < CNT_HIDDEVICES; i++) {
+    if (*hiddrivers[i] != hid_driver_active[i]) {
+      if (hid_driver_active[i]) {
+        Serial.printf("*** HID Device %s - disconnected ***\n", hid_driver_names[i]);
+        hid_driver_active[i] = false;
+      } else {
+        Serial.printf("*** HID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());
+        hid_driver_active[i] = true;
+
+        const uint8_t *psz = hiddrivers[i]->manufacturer();
+        if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
+        psz = hiddrivers[i]->product();
+        if (psz && *psz) Serial.printf("  product: %s\n", psz);
+        psz = hiddrivers[i]->serialNumber();
+        if (psz && *psz) Serial.printf("  Serial: %s\n", psz);
+
+        // See if this is our joystick object...
+        if (hiddrivers[i] == &joystick1) {
+          Serial.printf("  Joystick type: %d\n", joystick1.joystickType());
+          if (joystick1.joystickType() == JoystickController::PS3_MOTION) {
+            Serial.println("  PS3 Motion detected");
+            PS3_MOTION_timer = millis();  // set time for last event
+            PS3_MOTION_tried_to_pair_state = 0; 
+          }
+        }
+
+      }
+    }
+  }
+  // Then Bluetooth devices
+  for (uint8_t i = 0; i < CNT_BTHIDDEVICES; i++) {
+    if (*bthiddrivers[i] != bthid_driver_active[i]) {
+      if (bthid_driver_active[i]) {
+        Serial.printf("*** BTHID Device %s - disconnected ***\n", hid_driver_names[i]);
+        bthid_driver_active[i] = false;
+      } else {
+        Serial.printf("*** BTHID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());
+        bthid_driver_active[i] = true;
+
+        const uint8_t *psz = bthiddrivers[i]->manufacturer();
+        if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
+        psz = bthiddrivers[i]->product();
+        if (psz && *psz) Serial.printf("  product: %s\n", psz);
+        psz = bthiddrivers[i]->serialNumber();
+        if (psz && *psz) Serial.printf("  Serial: %s\n", psz);
+      }
+    }
+  }
+}
+
+//=============================================================================
+// displayPS4Data
+//=============================================================================
 void displayPS4Data()
 {
   buttons = joystick1.getButtons();
   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);
-  Serial.printf("Battery Status: %d\n", ((psAxis[30] & (1 << 4) - 1)*10));
+  Serial.printf("Battery Status: %d\n", ((psAxis[30] & ((1 << 4) - 1))*10));
   printAngles();
   Serial.println();
 
 
 }
 
+//=============================================================================
+// displayPS3Data
+//=============================================================================
 void displayPS3Data()
 
 {
     buttons_prev = buttons;
   }
 }
+//=============================================================================
+// displayPS3MotionData
+//=============================================================================
+void displayPS3MotionData()
+
+{
+  buttons = joystick1.getButtons();
+
+  // Hard to know what is best here. for now just copy raw data over... 
+  // will do this for now... Format of thought to be data.
+  //  data[1-3] Buttons (mentioned 4 as well but appears to be counter
+  // axis[0-1] data[5] Trigger, Previous trigger value
+  // 2-5 Unknown probably place holders for Axis like data for other PS3
+  // 6 - Time stamp
+  // 7 - Battery
+  // 8-19 - Accel: XL, XH, YL, YH, ZL, ZH, XL2, XH2, YL2, YH2, ZL2, ZH2
+  // 20-31 - Gyro: Xl,Xh,Yl,Yh,Zl,Zh,Xl2,Xh2,Yl2,Yh2,Zl2,Zh2
+  // 32 - Temp High
+  // 33 - Temp Low (4 bits)  Maybe Magneto x High on other?? 
+
+  // Use Select button to choose raw or not
+  if ((buttons & 0x01) && !(buttons_prev & 0x01)) show_raw_data = !show_raw_data;
+  if ((buttons & 0x04) && !(buttons_prev & 0x04)) show_changed_data = !show_changed_data;
+
+  if (show_raw_data)  {
+    displayRawData();
+  } else {
+    uint64_t changed_mask = joystick1.axisChangedMask();
+    Serial.printf("Changed: %08x Buttons: %x: Trig: %d\r\n", (uint32_t)changed_mask, buttons, psAxis[0]);
+    Serial.printf("Battery Status: %d\n", psAxis[7]);
+    printPS3MotionAngles();
+    Serial.println();
+  }
+
+  uint8_t ltv = psAxis[0];
 
+  if ((ltv != joystick_left_trigger_value) ) {
+    joystick_left_trigger_value = ltv;
+    Serial.printf("Rumbling: %d\r\n", ltv);
+    joystick1.setRumble(ltv, 0);
+  }
+
+  if (buttons != buttons_prev) {
+    uint8_t ledsR = (buttons & 0x8000)? 0xff : 0;   //Srq
+    uint8_t ledsG = (buttons & 0x2000)? 0xff : 0;   //Cir
+    uint8_t ledsB = (buttons & 0x1000)? 0xff : 0;   //Tri
+    Serial.printf("Set Leds %x %x %x\r\n", ledsR, ledsG, ledsB );
+    joystick1.setLEDs(ledsR, ledsG, ledsB);
+    buttons_prev = buttons;
+  }
+}
+
+//=============================================================================
+// displayXBoxData
+//=============================================================================
 void displayXBoxData()
 {
   buttons = joystick1.getButtons();
   }
 }
 
+//=============================================================================
+// displayRawData
+//=============================================================================
 void displayRawData() {
   uint64_t axis_mask = joystick1.axisMask();
   uint64_t changed_mask = joystick1.axisChangedMask();
 }
 
 
+//=============================================================================
+// OnReceiveHidData
+//=============================================================================
 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...
   }
 
   return true;
-}
+}
+
+//=============================================================================
+// processPS3MotionTimer
+//=============================================================================
+static const uint32_t PS3_MOTION_colors[] = {0, 0xff, 0xff00, 0xff0000, 0xffff, 0xff00ff, 0xffff00, 0xffffff};
+
+uint8_t PS3_MOTION_colors_index = 0;
+
+void processPS3MotionTimer() {
+  // See if we have a PS3_MOTION connected and we have run for a certain amount of time
+
+  if (PS3_MOTION_timer && ((millis()-PS3_MOTION_timer) >= PS3_MOTION_PERIOD)) {
+    Serial.println("PS3 Motion Timer"); Serial.flush();
+    if (joystick1) {
+      PS3_MOTION_timer = millis(); // joystick not there any more...
+
+      // We will first try to set feedback color for the PS3, maybe alternate colors
+      if (++PS3_MOTION_colors_index >= sizeof(PS3_MOTION_colors)/sizeof(PS3_MOTION_colors[0])) PS3_MOTION_colors_index = 0;
+      joystick1.setLEDs(PS3_MOTION_colors[PS3_MOTION_colors_index]);
+
+      // Next see if we can try to pair.
+      if (PS3_MOTION_tried_to_pair_state == 0) {
+        Serial.println("PS3_MOTION Connected");
+        if (!last_bdaddr[0] && !last_bdaddr[1] && !last_bdaddr[2] && !last_bdaddr[3] && !last_bdaddr[4] && !last_bdaddr[5]) {
+          Serial.println(" - No Bluetooth adapter has been plugged in - so will not try to pair");
+          PS3_MOTION_tried_to_pair_state = 1;
+        }
+      } 
+      if ((PS3_MOTION_tried_to_pair_state < 2)  &&
+          (last_bdaddr[0] || last_bdaddr[1] || last_bdaddr[2] || last_bdaddr[3] || last_bdaddr[4] || last_bdaddr[5])) {
+        Serial.println("  - Bluetooth device detected, will try to pair");
+        // Lets try to pair
+        if (! joystick1.PS3Pair(last_bdaddr)) {
+          Serial.println(" - Pairing call Failed");
+        } else {
+          Serial.println(" - Pairing complete (I hope), make sure Bluetooth adapter is plugged in and try PS3 without USB");
+        }
+        PS3_MOTION_tried_to_pair_state = 2; // don't try again...
+      }
+    }  else {
+      Serial.println("PS3 Motion Joystick no longer detected");
+      PS3_MOTION_timer = 0; // joystick not there any more...
+    }
+  }
+}

examples/Bluetooth/JoystickBT/helperPS.ino

 float pitch, roll;
 float gx, gy, gz;
+int16_t mx, my, mz;
 uint16_t xc, yc; 
 uint8_t isTouch;
 float ax, ay, az;
 int16_t xc_old, yc_old;
 
+/* Decode 12-bit signed value (assuming two's complement) */
+#define TWELVE_BIT_SIGNED(x) (((x) & 0x800)?(-(((~(x)) & 0xFFF) + 1)):(x))
+
+#define Model_ZCM1 1
+//#define Model_ZCM2 0
+
 void printAngles(){
   //test function calls
   float gx, gy, gz;
 void getCoords(uint16_t &xc, uint16_t &yc, uint8_t &isTouch){
 
 	//uint8_t finger = 0;  //only getting finger 1
-	uint8_t Id = 0;
-
 
   // Trackpad touch 1: id, active, x, y
   xc = ((psAxis[37] & 0x0f) << 8) | psAxis[36];
 	gy = (float) gyroy * RAD_TO_DEG/1024;
 	gz = (float) gyroz * RAD_TO_DEG/1024;
 }
+
+
+void printPS3MotionAngles(){
+  //test function calls
+  float gx, gy, gz;
+  getPS3MotionAccel(ax, ay, az);
+  Serial.printf("Accel-g's: %f, %f, %f\n", ax, ay, az);
+  getPS3MotionGyro(gx, gy, gz);
+  Serial.printf("Gyro-deg/sec: %f, %f, %f\n", gx, gy, gz);
+  getPS3MotionMag(mx, my, mz);
+  Serial.printf("Mag: %d, %d, %d\n", mx, my, mz);
+
+  getPS3MotionAngles(pitch, roll);
+  Serial.printf("Pitch/Roll: %f, %f\n", pitch, roll);
+}
+
+
+void getPS3MotionAccel( float &ax,  float &ay,  float &az){
+    int accelx = (psAxis[15]<<8 | psAxis[14]);
+    int accely = (psAxis[17]<<8 | psAxis[16]);
+    int accelz = (psAxis[19]<<8 | psAxis[18]);  
+  #if defined(Model_ZCM1)
+  	accelx = accelx-0x8000;
+  	accely = accely-0x8000;
+  	accelz = accelz-0x8000;
+  #elif defined(Model_ZCM2)
+    accelx = (accelx & 0x8000) ? (-(~accelx & 0xFFFF) + 1) : accelx;
+    accely = (accely & 0x8000) ? (-(~accely & 0xFFFF) + 1) : accely;
+    accelz = (accelz & 0x8000) ? (-(~accelz & 0xFFFF) + 1) : accelz;
+  #endif
+    ax = (float) accelx/4096;
+  	ay = (float) accely/4096;
+  	az = (float) accelz/4096;
+  
+}
+
+void getPS3MotionAngles(float &p, float &r){
+	getAccel( ax,  ay,  az);
+	p = (atan2f(ay, az) + PI) * RAD_TO_DEG;
+	r = (atan2f(ax, az) + PI) * RAD_TO_DEG;
+}
+
+void getPS3MotionGyro(float &gx, float &gy, float &gz){
+    int gyrox = (psAxis[21]<<8 | psAxis[20]);
+    int gyroy = (psAxis[23]<<8 | psAxis[22]);
+    int gyroz = (psAxis[25]<<8 | psAxis[24]);
+  #if defined(Model_ZCM1)
+  	gyrox = gyrox-0x8000;
+  	gyroy = gyroy-0x8000;
+  	gyroz = gyroz-0x8000;
+  #elif defined(Model_ZCM2)
+    gyrox = (gyrox & 0x8000) ? (-(~gyrox & 0xFFFF) + 1) : gyrox;
+    gyroy = (gyroy & 0x8000) ? (-(~gyroy & 0xFFFF) + 1) : gyroy;
+    gyroz = (gyroz & 0x8000) ? (-(~gyroz & 0xFFFF) + 1) : gyroz;
+  #endif
+
+  	gx = (float) gyrox * RAD_TO_DEG/1024;
+	gy = (float) gyroy * RAD_TO_DEG/1024;
+	gz = (float) gyroz * RAD_TO_DEG/1024;
+  
+}
 	
+void getPS3MotionMag(int16_t &mx, int16_t &my, int16_t &mz){
+  #if defined(Model_ZCM1)
+      mx = TWELVE_BIT_SIGNED(((psAxis[33] & 0x0F) << 8) |
+                 psAxis[34]);
+
+      my = TWELVE_BIT_SIGNED((psAxis[35] << 4) |
+                 (psAxis[36] & 0xF0) >> 4);
+
+      mz = TWELVE_BIT_SIGNED(((psAxis[36] & 0x0F) << 8) |
+                  psAxis[37]);
+  #elif defined(Model_ZCM2)
+      // NOTE: This model does not have magnetometers
+      Serial.println("Not avail for ZCM2!");
+  #endif
+}

examples/USBHost_viewer/USBHost_viewer.ino

     if (*bthiddrivers[i] != bthid_driver_active[i]) {
       if (bthid_driver_active[i]) {
         Serial.printf("*** BTHID Device %s - disconnected ***\n", hid_driver_names[i]);
-        hid_driver_active[i] = false;
+        bthid_driver_active[i] = false;
       } else {
         new_device_detected = true;
         Serial.printf("*** BTHID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());

hid.cpp

 		parse();
 		queue_Data_Transfer(in_pipe, report, in_size, this);
 		if (device->idVendor == 0x054C && 
-				((device->idProduct == 0x0268) || (device->idProduct == 0x042F) || (device->idProduct == 0x03D5))) {
+				((device->idProduct == 0x0268) || (device->idProduct == 0x042F)/* || (device->idProduct == 0x03D5)*/)) {
 			println("send special PS3 feature command");
 			mk_setup(setup, 0x21, 9, 0x03F4, 0, 4); // ps3 tell to send report 1?
 			static uint8_t ps3_feature_F4_report[] = {0x42, 0x0c, 0x00, 0x00};

joystick.cpp

 	{ 0x045e, 0x0719, XBOX360, false},
 	{ 0x054C, 0x0268, PS3, true}, 
 	{ 0x054C, 0x042F, PS3, true},	// PS3 Navigation controller
-	{ 0x054C, 0x03D5, PS3, true},	// PS3 Motion controller
+	{ 0x054C, 0x03D5, PS3_MOTION, true},	// PS3 Motion controller
 	{ 0x054C, 0x05C4, PS4, true}, {0x054C, 0x09CC, PS4, true }
 };
 
 			break;
 		case PS3:
 			return transmitPS3UserFeedbackMsg();
+		case PS3_MOTION:
+			return transmitPS3MotionUserFeedbackMsg();
 		case PS4:
 			return transmitPS4UserFeedbackMsg();
 		case XBOXONE:
 	return false;
 }
 
+
 bool JoystickController::setLEDs(uint8_t lr, uint8_t lg, uint8_t lb)
 {
 	// Need to know which joystick we are on.  Start off with XBox support - maybe need to add some enum value for the known
 		switch (joystickType_) {
 			case PS3:
 				return transmitPS3UserFeedbackMsg();
+			case PS3_MOTION:
+				return transmitPS3MotionUserFeedbackMsg();
 			case PS4:
 				return transmitPS4UserFeedbackMsg();
 			case XBOX360:
 	    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 
+	    txbuf_[9] = leds_[2] << 1; // RGB value 	// using third led now...
 	    //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_[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 
+	    txbuf_[11] = leds_[2] << 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;
 }
 
+#define MOVE_REPORT_BUFFER_SIZE 7
+#define MOVE_HID_BUFFERSIZE 50 // Size of the buffer for the Playstation Motion Controller
+
+bool JoystickController::transmitPS3MotionUserFeedbackMsg() {
+	if (driver_) {
+        txbuf_[0] = 0x02; // Set report ID, this is needed for Move commands to work
+        txbuf_[2] = leds_[0];
+        txbuf_[3] = leds_[1];
+        txbuf_[4] = leds_[2];
+        txbuf_[6] = rumble_lValue_; // Set the rumble value into the write buffer
+
+		//return driver_->sendControlPacket(0x21, 9, 0x201, 0, MOVE_REPORT_BUFFER_SIZE, txbuf_); 
+		return driver_->sendPacket(txbuf_, MOVE_REPORT_BUFFER_SIZE);
+
+	} else if (btdriver_) {
+        txbuf_[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
+        txbuf_[1] = 0x02; // Report ID
+        txbuf_[3] = leds_[0];
+        txbuf_[4] = leds_[1];
+        txbuf_[5] = leds_[2];
+        txbuf_[7] = rumble_lValue_;
+     	btdriver_->sendL2CapCommand(txbuf_, MOVE_HID_BUFFERSIZE, BluetoothController::INTERRUPT_SCID);
+     	return true;
+	}
+	return false;
+}
+
 //*****************************************************************************
 // Support for Joysticks that Use HID data. 
 //*****************************************************************************
 	DBGPrintf("JoystickController::claim_collection joystickType_=%d\n", joystickType_);
 	switch (joystickType_) {
 		case PS3:
+		case PS3_MOTION: // not sure yet
 			additional_axis_usage_page_ = 0x1;
 			additional_axis_usage_start_ = 0x100;
 			additional_axis_usage_count_ = 39;
 	}
 
 	if (remoteName && mapNameToJoystickType(remoteName)) {
-		if (joystickType_ == PS3) {
+		if ((joystickType_ == PS3) || (joystickType_ == PS3_MOTION)) {
 			DBGPrintf("JoystickController::claim_bluetooth TRUE PS3 hack...\n");
 			btdriver_ = driver;
 			btdevice = (Device_t*)driver;	// remember this way 
 	// Example data from PS4 controller
 	//01 7e 7f 82 84 08 00 00 00 00
 	//   LX LY RX RY BT BT PS LT RT
-	//DBGPrintf("JoystickController::process_bluetooth_HID_data\n");
+	DBGPrintf("JoystickController::process_bluetooth_HID_data: data[0]=%x\n", data[0]);
 	// May have to look at this one with other controllers...
 	if (data[0] == 1) {
 		//print("  Joystick Data: ");
-		//print_hexbytes(data, length);
-//		DBGPrintf("  Joystick Data: ");
+		// print_hexbytes(data, length);
 		if (length > TOTAL_AXIS_COUNT) length = TOTAL_AXIS_COUNT;	// don't overflow arrays...
+		DBGPrintf("  Joystick Data: ");
+		for(uint16_t i =0; i < length; i++) DBGPrintf("%02x ", data[i]);
+		DBGPrintf("\r\n");	
 		if (joystickType_ == PS3) {
 			// Quick and dirty hack to match PS3 HID data
 			uint32_t cur_buttons = data[2] | ((uint16_t)data[3] << 8) | ((uint32_t)data[4] << 16); 
 				} 
 				mask <<= 1;	// shift down the mask.
 			}
+		} else if (joystickType_ == PS3_MOTION) {
+			// Quick and dirty PS3_Motion data.
+			uint32_t cur_buttons = data[1] | ((uint16_t)data[2] << 8) | ((uint32_t)data[3] << 16); 
+			if (cur_buttons != buttons) {
+				buttons = cur_buttons;
+				joystickEvent = true;	// something changed.
+			}
+
+			// Hard to know what is best here. for now just copy raw data over... 
+			// will do this for now... Format of thought to be data.
+			//  data[1-3] Buttons (mentioned 4 as well but appears to be counter
+			// axis[0-1] data[5] Trigger, Previous trigger value
+			// 2-5 Unknown probably place holders for Axis like data for other PS3
+			// 6 - Time stamp
+			// 7 - Battery
+			// 8-19 - Accel: XL, XH, YL, YH, ZL, ZH, XL2, XH2, YL2, YH2, ZL2, ZH2
+			// 20-31 - Gyro: Xl,Xh,Yl,Yh,Zl,Zh,Xl2,Xh2,Yl2,Yh2,Zl2,Zh2
+			// 32 - Temp High
+			// 33 - Temp Low (4 bits)  Maybe Magneto x High on other?? 
+			uint64_t mask = 0x1;
+			axis_mask_ = 0;	// assume bits 0, 1, 2, 5
+			// Then rest of data
+			mask = 0x1 << 10;	// setup for other bits
+			for (uint16_t i = 5; i < length; i++ ) {
+				axis_mask_ |= mask;
+				if(data[i] != axis[i-5]) { 
+					axis_changed_mask_ |= mask;
+					axis[i-5] = data[i];
+				} 
+				mask <<= 1;	// shift down the mask.
+			}
 
 		} else {
 			uint64_t mask = 0x1;
 		return true;
 
 	} else if(data[0] == 0x11){
-		if (data[0] == 1) {
-			//print("  Joystick Data: ");
-			//print_hexbytes(data, length);
-	//		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_changed_mask_ |= mask;
-					axis[i] = data[i];
-				} 
-				mask <<= 1;	// shift down the mask.
-	//			DBGPrintf("%02x ", axis[i]);
-			}
-	//		DBGPrintf("\n");
-			joystickEvent = true;
-			connected_ = true;
-			return true;
-		} else if(data[0] == 0x11){
-			DBGPrintf("\n  Joystick Data: ");
-			uint64_t mask = 0x1;
-			axis_mask_ = 0;
-			axis_changed_mask_ = 0;
-			
-			//This moves data to be equivalent to what we see for
-			//data[0] = 0x01
-			uint8_t tmp_data[length-2];
-			
-			for (uint16_t i = 0; i < (length-2); i++ ) {
-				tmp_data[i] = 0;
-				tmp_data[i] = data[i+2];
-			}
-			
-			/*
-			 * [1] LX, [2] = LY, [3] = RX, [4] = RY
-			 * [5] combo, tri, cir, x, sqr, D-PAD (4bits, 0-3
-			 * [6] R3,L3, opt, share, R2, L2, R1, L1
-			 * [7] Counter (bit7-2), T-PAD, PS
-			 * [8] Left Trigger, [9] Right Trigger
-			 * [10-11] Timestamp
-			 * [12] Battery (0 to 0xff)
-			 * [13-14] acceleration x
-			 * [15-16] acceleration y
-			 * [17-18] acceleration z
-			 * [19-20] gyro x
-			 * [21-22] gyro y
-			 * [23-24] gyro z
-			 * [25-29] unknown
-			 * [30] 0x00,phone,mic, usb, battery level (4bits)
-			 * rest is trackpad?  to do implement?
-			 */
-			//PS Bit
-			tmp_data[7] = (tmp_data[7] >> 0) & 1;
-			//set arrow buttons to axis[0]
-			tmp_data[10] = tmp_data[5] & ((1 << 4) - 1);
-			//set buttons for last 4bits in the axis[5]
-			tmp_data[5] = tmp_data[5] >> 4;
-			
+		DBGPrintf("\n  Joystick Data: ");
+		uint64_t mask = 0x1;
+		axis_mask_ = 0;
+		axis_changed_mask_ = 0;
 		
-			// Quick and dirty hack to match PS4 HID data
-			uint32_t cur_buttons = tmp_data[7] | (tmp_data[10]) | ((tmp_data[6]*10)) | ((uint16_t)tmp_data[5] << 16) ; 
-			if (cur_buttons != buttons) {
-				buttons = cur_buttons;
-				joystickEvent = true;	// something changed.
-			}
-			
-			mask = 0x1;
-			axis_mask_ = 0x27;	// assume bits 0, 1, 2, 5
-			for (uint16_t i = 0; i < 3; i++) {
-				if (axis[i] != tmp_data[i+1]) {
-					axis_changed_mask_ |= mask;
-					axis[i] = tmp_data[i+1];
-				}
-				mask <<= 1;	// shift down the mask.
-			}
-			if (axis[5] != tmp_data[4]) {
-				axis_changed_mask_ |= (1<<5);
-				axis[5] = tmp_data[4];
-			}
-			
-			if (axis[3] != tmp_data[8]) {
-				axis_changed_mask_ |= (1<<3);
-				axis[3] = tmp_data[8];
-			}
-			
-			if (axis[4] != tmp_data[9]) {
-				axis_changed_mask_ |= (1<<4);
-				axis[4] = tmp_data[9];
+		//This moves data to be equivalent to what we see for
+		//data[0] = 0x01
+		uint8_t tmp_data[length-2];
+		
+		for (uint16_t i = 0; i < (length-2); i++ ) {
+			tmp_data[i] = 0;
+			tmp_data[i] = data[i+2];
+		}
+		
+		/*
+		 * [1] LX, [2] = LY, [3] = RX, [4] = RY
+		 * [5] combo, tri, cir, x, sqr, D-PAD (4bits, 0-3
+		 * [6] R3,L3, opt, share, R2, L2, R1, L1
+		 * [7] Counter (bit7-2), T-PAD, PS
+		 * [8] Left Trigger, [9] Right Trigger
+		 * [10-11] Timestamp
+		 * [12] Battery (0 to 0xff)
+		 * [13-14] acceleration x
+		 * [15-16] acceleration y
+		 * [17-18] acceleration z
+		 * [19-20] gyro x
+		 * [21-22] gyro y
+		 * [23-24] gyro z
+		 * [25-29] unknown
+		 * [30] 0x00,phone,mic, usb, battery level (4bits)
+		 * rest is trackpad?  to do implement?
+		 */
+		//PS Bit
+		tmp_data[7] = (tmp_data[7] >> 0) & 1;
+		//set arrow buttons to axis[0]
+		tmp_data[10] = tmp_data[5] & ((1 << 4) - 1);
+		//set buttons for last 4bits in the axis[5]
+		tmp_data[5] = tmp_data[5] >> 4;
+		
+	
+		// Quick and dirty hack to match PS4 HID data
+		uint32_t cur_buttons = tmp_data[7] | (tmp_data[10]) | ((tmp_data[6]*10)) | ((uint16_t)tmp_data[5] << 16) ; 
+		if (cur_buttons != buttons) {
+			buttons = cur_buttons;
+			joystickEvent = true;	// something changed.
+		}
+		
+		mask = 0x1;
+		axis_mask_ = 0x27;	// assume bits 0, 1, 2, 5
+		for (uint16_t i = 0; i < 3; i++) {
+			if (axis[i] != tmp_data[i+1]) {
+				axis_changed_mask_ |= mask;
+				axis[i] = tmp_data[i+1];
 			}
-			
-			//limit for masking
-			mask = 0x1;
-			for (uint16_t i = 6; i < (64); i++ ) {
-				axis_mask_ |= mask;
-				if(tmp_data[i] != axis[i]) { 
-					axis_changed_mask_ |= mask;
-					axis[i] = tmp_data[i];
-				}
-				mask <<= 1;	// shift down the mask.
-				DBGPrintf("%02x ", axis[i]);
+			mask <<= 1;	// shift down the mask.
+		}
+		if (axis[5] != tmp_data[4]) {
+			axis_changed_mask_ |= (1<<5);
+			axis[5] = tmp_data[4];
+		}
+		
+		if (axis[3] != tmp_data[8]) {
+			axis_changed_mask_ |= (1<<3);
+			axis[3] = tmp_data[8];
+		}
+		
+		if (axis[4] != tmp_data[9]) {
+			axis_changed_mask_ |= (1<<4);
+			axis[4] = tmp_data[9];
+		}
+		
+		//limit for masking
+		mask = 0x1;
+		for (uint16_t i = 6; i < (64); i++ ) {
+			axis_mask_ |= mask;
+			if(tmp_data[i] != axis[i]) { 
+				axis_changed_mask_ |= mask;
+				axis[i] = tmp_data[i];
 			}
-			DBGPrintf("\n");
-			//DBGPrintf("Axis Mask (axis_mask_, axis_changed_mask_; %d, %d\n", axis_mask_,axis_changed_mask_);
-			joystickEvent = true;
-			connected_ = true;
+			mask <<= 1;	// shift down the mask.
+			DBGPrintf("%02x ", axis[i]);
 		}
+		DBGPrintf("\n");
+		//DBGPrintf("Axis Mask (axis_mask_, axis_changed_mask_; %d, %d\n", axis_mask_,axis_changed_mask_);
+		joystickEvent = true;
+		connected_ = true;
 	}
 	return false;
 }
 	} else if (strncmp((const char *)remoteName, "Navigation Controller", 21) == 0) {
 		DBGPrintf("  JoystickController::mapNameToJoystickType %x %s - set to PS3\n", (uint32_t)this, remoteName);
 		joystickType_ = PS3;
+	} else if (strncmp((const char *)remoteName, "Motion Controller", 17) == 0) {
+		DBGPrintf("  JoystickController::mapNameToJoystickType %x %s - set to PS3 Motion\n", (uint32_t)this, remoteName);
+		joystickType_ = PS3_MOTION;
 	} else if (strncmp((const char *)remoteName, "Xbox Wireless", 13) == 0) {
 		DBGPrintf("  JoystickController::mapNameToJoystickType %x %s - set to XBOXONE\n", (uint32_t)this, remoteName);
 		joystickType_ = XBOXONE;
 		switch (joystickType_) {
 			case PS4: special_process_required = SP_NEED_CONNECT; break;
 			case PS3: special_process_required = SP_PS3_IDS; break;
+			case PS3_MOTION: special_process_required = SP_PS3_IDS; break;
 			default: 
 				break;
 		}
 void JoystickController::connectionComplete() 
 {
 	DBGPrintf("  JoystickController::connectionComplete %x joystick type %d\n", (uint32_t)this, 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);
+	switch (joystickType_) {
+	case 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);			
+		}
+		break;
+	case 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);
+		}
+		break;
+	case PS3_MOTION:	
+		setLEDs(0, 0xff, 0);	// Maybe try setting to green? 
+	default:
+		break;	
 	}
-
 }
 
 void JoystickController::release_bluetooth() 
 
 
 bool JoystickController::PS3Pair(uint8_t* bdaddr) {
- 	if ((joystickType_ != PS3) || !driver_) return false; // not a PS2 nor plugged into USB...
-
-    /* Set the internal Bluetooth address */
-    txbuf_[0] = 0x01;
-    txbuf_[1] = 0x00;
-
-    for(uint8_t i = 0; i < 6; i++)
-            txbuf_[i + 2] = bdaddr[5 - i]; // Copy into buffer, has to be written reversed, so it is MSB first
-
-    // 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
-	return driver_->sendControlPacket(0x21, 9, 0x3f5, 0, 8, txbuf_); 
+	if (!driver_) return false;
+ 	if (joystickType_ == PS3) {
+ 	    /* Set the internal Bluetooth address */
+	    txbuf_[0] = 0x01;
+	    txbuf_[1] = 0x00;
+
+	    for(uint8_t i = 0; i < 6; i++)
+	            txbuf_[i + 2] = bdaddr[5 - i]; // Copy into buffer, has to be written reversed, so it is MSB first
+
+	    // 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
+		return driver_->sendControlPacket(0x21, 9, 0x3f5, 0, 8, txbuf_); 
+	} else if (joystickType_ == PS3_MOTION) {
+		// Slightly different than other PS3 units...
+	    txbuf_[0] = 0x05;
+	    for(uint8_t i = 0; i < 6; i++)
+	            txbuf_[i + 1] = bdaddr[i]; // Order different looks like LSB First?
+
+        txbuf_[7] = 0x10;
+        txbuf_[8] = 0x01;
+        txbuf_[9] = 0x02;
+        txbuf_[10] = 0x12;
+	    // 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
+		return driver_->sendControlPacket(0x21, 9, 0x305, 0, 11, txbuf_); 
+	}
+	return false;
 }

keywords.txt

 setLEDs	KEYWORD2
 joystickType	KEYWORD2
 PS3	LITERAL1
+PS3_MOTION	LITERAL1
 PS4	LITERAL1
 XBOXONE	LITERAL1
 XBOX360	LITERAL1

mouse.cpp

 hidclaim_t MouseController::claim_collection(USBHIDParser *driver, Device_t *dev, uint32_t topusage)
 {
 	// only claim Desktop/Mouse
-	if (topusage != 0x10002) return CLAIM_NO;
+	if ((topusage != 0x10002) && (topusage != 0x10001)) return CLAIM_NO;
 	// only claim from one physical device
 	if (mydevice != NULL && dev != mydevice) return CLAIM_NO;
 	mydevice = dev;