Merge pull request #5 from mjs513/WIP2-Bluetooth

Update to joystick and example

examples/JoystickBT/JoystickBT.ino

   Serial4.println("\n" __FILE__ " " __DATE__ " " __TIME__);
   Serial4.println("\n********\n T4 connected Serial4 *******\n");
 */
-Serial1.begin(2000000);
+Serial1.begin(1843200);
   while (!Serial) ; // wait for Arduino Serial Monitor
   //debTraceShow_tt( -2, "", "", "" );
   //Serial4.println("\n" __FILE__ " " __DATE__ " " __TIME__);
       //}
       //Serial.println();
         
-
+     delay(100);
      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);
-
+  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);
+  printAngles();
   Serial.println();
 
   uint8_t ltv;
   uint8_t rtv;
 
-    ltv = psAxis[8];
-    rtv = psAxis[9];
+    ltv = psAxis[3];
+    rtv = psAxis[4];
 
     if ((ltv != joystick_left_trigger_value) || (rtv != joystick_right_trigger_value)) {
       joystick_left_trigger_value = ltv;
       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
       Serial.print(lr); Serial.print(", "); 
       Serial.print(lg); Serial.print(", "); 
       Serial.println(lb); 
-
       joystick1.setLEDs(lr, lg, lb);
-      buttons_prev =psAxis[5];
+      buttons_prev =psAxis[5];  
   }
+*/
 }
 
 void displayPS3Data()

examples/JoystickBT/helperPS.ino

+float pitch, roll;
+float gx, gy, gz;
+uint16_t xc, yc; 
+uint8_t isTouch;
+float ax, ay, az;
+int16_t xc_old, yc_old;
+
+void printAngles(){
+  //test function calls
+  float gx, gy, gz;
+  getAccel(ax, ay, az);
+  Serial.printf("Accel-g's: %f, %f, %f\n", ax, ay, az);
+  getGyro(gx, gy, gz);
+  Serial.printf("Gyro-deg/sec: %f, %f, %f\n", gx, gy, gz);
+
+  getAngles(pitch, roll);
+  Serial.printf("Pitch/Roll: %f, %f\n", pitch, roll);
+
+  getCoords(xc, yc, isTouch);
+}
+
+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];
+  yc = psAxis[38] << 4 | ((psAxis[37] & 0xf0) >> 4),
+
+	isTouch = psAxis[35] >> 7;
+  if(xc != xc_old || yc != yc_old){
+    Serial.printf("Touch: %d, %d, %d, %d\n", psAxis[33], isTouch, xc, yc);
+    xc_old = xc;
+    yc_old = yc;
+  }
+}
+
+void getAccel( float &ax,  float &ay,  float &az){
+	int accelx = (int16_t)(psAxis[20]<<8) | psAxis[19];
+	int accelz = (int16_t)(psAxis[22]<<8) | psAxis[21];
+	int accely = (int16_t)(psAxis[24]<<8) | psAxis[23];
+
+	ax = (float) accelx/8192;
+	ay = (float) accely/8192;
+	az = (float) accelz/8192;
+}
+
+void getAngles(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 getGyro(float &gx, float &gy, float &gz){
+	int gyroy = (int16_t)(psAxis[14]<<8) | psAxis[13];
+	int gyroz = (int16_t)(psAxis[16]<<8) | psAxis[15];
+	int gyrox = (int16_t)(psAxis[18]<<8) | psAxis[17];
+
+	gx = (float) gyrox * RAD_TO_DEG/1024;
+	gy = (float) gyroy * RAD_TO_DEG/1024;
+	gz = (float) gyroz * RAD_TO_DEG/1024;
+}
+	

joystick.cpp

 				axis_changed_mask_ |= (1<<5);
 				axis[5] = data[9];
 			}
-
+			
+			if (axis[3] != data[18]) {
+				axis_changed_mask_ |= (1<<3);
+				axis[3] = data[18];
+			}
+			
+			if (axis[4] != data[19]) {
+				axis_changed_mask_ |= (1<<4);
+				axis[4] = data[19];
+			}
+			
 			// Then rest of data
 			mask = 0x1 << 10;	// setup for other bits
 			for (uint16_t i = 10; i < length; i++ ) {
 		return true;
 
 	} else if(data[0] == 0x11){
-		DBGPrintf("  Joystick Data: ");
-		uint64_t mask = 0x1;
-		axis_mask_ = 0;
-		axis_changed_mask_ = 0;
+		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;
+			
 		
-		//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] = data[i+2];
-			DBGPrintf("%02x ",tmp_data[i]);
-		}
-		DBGPrintf("\n");		
-		/*
-		 * [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[0] = tmp_data[5] & ((1 << 4) - 1);
-		//set buttons for last 4bits in the axis[5]
-		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.
-			//DBGPrintf("%02x ", axis[i]);
+			// 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];
+			}
+			
+			//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]);
+			}
+			DBGPrintf("\n");
+			//DBGPrintf("Axis Mask (axis_mask_, axis_changed_mask_; %d, %d\n", axis_mask_,axis_changed_mask_);
+			joystickEvent = true;
+			connected_ = true;
 		}
-		//DBGPrintf("\n");
-		joystickEvent = true;
-		connected_ = true;
 	}
 	return false;
 }