visus
/
teensy-usbhost
보기 1
좋아요 0
포크 0
코드 이슈 0 풀 리퀘스트 0 릴리즈 0 위키 Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
287 커밋
1 브렌치
트리: 93f0ea36fd
브랜치 태그
${ item.name }
Create branch ${ searchTerm }
from '93f0ea36fd'
${ noResults }
teensy-usbhost/joystick.cpp

1142 lines
38KB
Raw Blame 히스토리 

  1. /* USB EHCI Host for Teensy 3.6

  2.  * Copyright 2017 Paul Stoffregen (paul@pjrc.com)

  3.  *

  4.  * Permission is hereby granted, free of charge, to any person obtaining a

  5.  * copy of this software and associated documentation files (the

  6.  * "Software"), to deal in the Software without restriction, including

  7.  * without limitation the rights to use, copy, modify, merge, publish,

  8.  * distribute, sublicense, and/or sell copies of the Software, and to

  9.  * permit persons to whom the Software is furnished to do so, subject to

  10.  * the following conditions:

  11.  *

  12.  * The above copyright notice and this permission notice shall be included

  13.  * in all copies or substantial portions of the Software.

  14.  *

  15.  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS

  16.  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF

  17.  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.

  18.  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY

  19.  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,

  20.  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE

  21.  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

  22.  */

  23. 

  24. #include <Arduino.h>

  25. #include "USBHost_t36.h"  // Read this header first for key info

  26. 

  27. #define print   USBHost::print_

  28. #define println USBHost::println_

  29. 

  30. //#define DEBUG_JOYSTICK

  31. #ifdef  DEBUG_JOYSTICK

  32. #define DBGPrintf USBHDBGSerial.printf

  33. #else

  34. #define DBGPrintf(...) 

  35. #endif

  36. 

  37. // PID/VID to joystick mapping - Only the XBOXOne is used to claim the USB interface directly, 

  38. // The others are used after claim-hid code to know which one we have and to use it for 

  39. // doing other features.  

  40. JoystickController::product_vendor_mapping_t JoystickController::pid_vid_mapping[] = {

  41. 	{ 0x045e, 0x02ea, XBOXONE, false },{ 0x045e, 0x02dd, XBOXONE, false },

  42. 	{ 0x045e, 0x0719, XBOX360, false},

  43. 	{ 0x045e, 0x028E, SWITCH, false},  // Switch? 

  44. 	{ 0x054C, 0x0268, PS3, true}, 

  45. 	{ 0x054C, 0x042F, PS3, true},	// PS3 Navigation controller

  46. 	{ 0x054C, 0x03D5, PS3_MOTION, true},	// PS3 Motion controller

  47. 	{ 0x054C, 0x05C4, PS4, true}, 	{0x054C, 0x09CC, PS4, true },

  48. 	{ 0x046D, 0xC626, SpaceNav, true},  // 3d Connextion Space Navigator, 0x10008

  49. 	{ 0x046D, 0xC628, SpaceNav, true}  // 3d Connextion Space Navigator, 0x10008

  50. };

  51. 

  52. 

  53. 

  54. //-----------------------------------------------------------------------------

  55. void JoystickController::init()

  56. {

  57. 	contribute_Pipes(mypipes, sizeof(mypipes)/sizeof(Pipe_t));

  58. 	contribute_Transfers(mytransfers, sizeof(mytransfers)/sizeof(Transfer_t));

  59. 	contribute_String_Buffers(mystring_bufs, sizeof(mystring_bufs)/sizeof(strbuf_t));

  60. 	driver_ready_for_device(this);

  61. 	USBHIDParser::driver_ready_for_hid_collection(this);

  62. 	BluetoothController::driver_ready_for_bluetooth(this);

  63. }

  64. 

  65. //-----------------------------------------------------------------------------

  66. JoystickController::joytype_t JoystickController::mapVIDPIDtoJoystickType(uint16_t idVendor, uint16_t idProduct, bool exclude_hid_devices)

  67. {

  68. 	for (uint8_t i = 0; i < (sizeof(pid_vid_mapping)/sizeof(pid_vid_mapping[0])); i++) {

  69. 		if ((idVendor == pid_vid_mapping[i].idVendor) && (idProduct == pid_vid_mapping[i].idProduct)) {

  70. 			println("Match PID/VID: ", i, DEC);

  71. 			if (exclude_hid_devices && pid_vid_mapping[i].hidDevice) return UNKNOWN;

  72. 			return pid_vid_mapping[i].joyType;

  73. 		}

  74. 	}  

  75. 	return UNKNOWN; 	// Not in our list

  76. }

  77. 

  78. //*****************************************************************************

  79. // Some simple query functions depend on which interface we are using...

  80. //*****************************************************************************

  81. 

  82. uint16_t JoystickController::idVendor() 

  83. {

  84. 	if (device != nullptr) return device->idVendor;

  85. 	if (mydevice != nullptr) return mydevice->idVendor;

  86. 	return 0;

  87. }

  88. 

  89. uint16_t JoystickController::idProduct() 

  90. {

  91. 	if (device != nullptr) return device->idProduct;

  92. 	if (mydevice != nullptr) return mydevice->idProduct;

  93. 	return 0;

  94. }

  95. 

  96. const uint8_t *JoystickController::manufacturer()

  97. {

  98. 	if ((device != nullptr) && (device->strbuf != nullptr)) return &device->strbuf->buffer[device->strbuf->iStrings[strbuf_t::STR_ID_MAN]];

  99. 	//if ((btdevice != nullptr) && (btdevice->strbuf != nullptr)) return &btdevice->strbuf->buffer[btdevice->strbuf->iStrings[strbuf_t::STR_ID_MAN]]; 

  100. 	if ((mydevice != nullptr) && (mydevice->strbuf != nullptr)) return &mydevice->strbuf->buffer[mydevice->strbuf->iStrings[strbuf_t::STR_ID_MAN]]; 

  101. 	return nullptr;

  102. }

  103. 

  104. const uint8_t *JoystickController::product()

  105. {

  106. 	if ((device != nullptr) && (device->strbuf != nullptr)) return &device->strbuf->buffer[device->strbuf->iStrings[strbuf_t::STR_ID_PROD]];

  107. 	if ((mydevice != nullptr) && (mydevice->strbuf != nullptr)) return &mydevice->strbuf->buffer[mydevice->strbuf->iStrings[strbuf_t::STR_ID_PROD]]; 

  108. 	if (btdevice != nullptr) return remote_name_;

  109. 	return nullptr;

  110. }

  111. 

  112. const uint8_t *JoystickController::serialNumber()

  113. {

  114. 	if ((device != nullptr) && (device->strbuf != nullptr)) return &device->strbuf->buffer[device->strbuf->iStrings[strbuf_t::STR_ID_SERIAL]];

  115. 	if ((mydevice != nullptr) && (mydevice->strbuf != nullptr)) return &mydevice->strbuf->buffer[mydevice->strbuf->iStrings[strbuf_t::STR_ID_SERIAL]]; 

  116. 	return nullptr;

  117. }

  118. 

  119. 

  120. bool JoystickController::setRumble(uint8_t lValue, uint8_t rValue, uint8_t timeout)

  121. {

  122. 	// Need to know which joystick we are on.  Start off with XBox support - maybe need to add some enum value for the known

  123. 	// joystick types. 

  124. 	rumble_lValue_ = lValue; 

  125. 	rumble_rValue_ = rValue;

  126. 	rumble_timeout_ = timeout;

  127. 

  128. 	switch (joystickType_) {

  129. 		default:

  130. 			break;

  131. 		case PS3:

  132. 			return transmitPS3UserFeedbackMsg();

  133. 		case PS3_MOTION:

  134. 			return transmitPS3MotionUserFeedbackMsg();

  135. 		case PS4:

  136. 			return transmitPS4UserFeedbackMsg();

  137. 		case XBOXONE:

  138. 			// Lets try sending a request to the XBox 1.

  139. 			txbuf_[0] = 0x9;

  140. 			txbuf_[1] = 0x0;

  141. 			txbuf_[2] = 0x0;

  142. 			txbuf_[3] = 0x09; // Substructure (what substructure rest of this packet has)

  143. 			txbuf_[4] = 0x00; // Mode

  144. 			txbuf_[5] = 0x0f; // Rumble mask (what motors are activated) (0000 lT rT L R)

  145. 			txbuf_[6] = 0x0; // lT force

  146. 			txbuf_[7] = 0x0; // rT force

  147. 			txbuf_[8] = lValue; // L force

  148. 			txbuf_[9] = rValue; // R force

  149. 			txbuf_[10] = 0xff; // Length of pulse

  150. 			txbuf_[11] = 0x00; // Period between pulses			

  151. 			txbuf_[12] = 0x00; // Repeat			

  152. 			if (!queue_Data_Transfer(txpipe_, txbuf_, 13, this)) {

  153. 				println("XBoxOne rumble transfer fail");

  154. 			}

  155. 			return true;	// 

  156. 		case XBOX360:

  157. 			txbuf_[0] = 0x00;

  158. 			txbuf_[1] = 0x01;

  159. 			txbuf_[2] = 0x0F;

  160. 			txbuf_[3] = 0xC0;

  161. 			txbuf_[4] = 0x00;

  162. 			txbuf_[5] = lValue;

  163. 			txbuf_[6] = rValue;

  164. 			txbuf_[7] = 0x00;

  165. 			txbuf_[8] = 0x00;

  166. 			txbuf_[9] = 0x00;

  167. 			txbuf_[10] = 0x00;

  168. 			txbuf_[11] = 0x00;

  169. 			if (!queue_Data_Transfer(txpipe_, txbuf_, 12, this)) {

  170. 				println("XBox360 rumble transfer fail");

  171. 			}

  172. 			return true;

  173. 	} 

  174. 	return false;

  175. }

  176. 

  177. 

  178. bool JoystickController::setLEDs(uint8_t lr, uint8_t lg, uint8_t lb)

  179. {

  180. 	// Need to know which joystick we are on.  Start off with XBox support - maybe need to add some enum value for the known

  181. 	// joystick types. 

  182. 	if ((leds_[0] != lr) || (leds_[1] != lg) || (leds_[2] != lb)) {

  183. 		leds_[0] = lr;

  184. 		leds_[1] = lg;

  185. 		leds_[2] = lb;

  186. 

  187. 		switch (joystickType_) {

  188. 			case PS3:

  189. 				return transmitPS3UserFeedbackMsg();

  190. 			case PS3_MOTION:

  191. 				return transmitPS3MotionUserFeedbackMsg();

  192. 			case PS4:

  193. 				return transmitPS4UserFeedbackMsg();

  194. 			case XBOX360:

  195. 				// 0: off, 1: all blink then return to before

  196. 				// 2-5(TL, TR, BL, BR) - blink on then stay on

  197. 				// 6-9() - On 

  198. 			    // ...

  199. 				txbuf_[1] = 0x00;

  200. 				txbuf_[2] = 0x08;

  201. 				txbuf_[3] = 0x40 + lr;

  202. 				txbuf_[4] = 0x00;

  203. 				txbuf_[5] = 0x00;

  204. 				txbuf_[6] = 0x00;

  205. 				txbuf_[7] = 0x00;

  206. 				txbuf_[8] = 0x00;

  207. 				txbuf_[9] = 0x00;

  208. 				txbuf_[10] = 0x00;

  209. 				txbuf_[11] = 0x00;

  210. 				if (!queue_Data_Transfer(txpipe_, txbuf_, 12, this)) {

  211. 					println("XBox360 set leds fail");

  212. 				}

  213. 				return true;

  214. 			case XBOXONE:

  215. 			default:

  216. 				return false;

  217. 		} 

  218. 	}

  219. 	return false;

  220. }

  221. 

  222. bool JoystickController::transmitPS4UserFeedbackMsg() {

  223. 	if (driver_)  {

  224. 		uint8_t packet[32];

  225. 	    memset(packet, 0, sizeof(packet));

  226. 

  227. 	    packet[0] = 0x05; // Report ID

  228. 	    packet[1]= 0xFF;

  229. 

  230. 	    packet[4] = rumble_lValue_; // Small Rumble

  231. 	    packet[5] = rumble_rValue_; // Big rumble

  232. 	    packet[6] = leds_[0]; // RGB value 

  233. 	    packet[7] = leds_[1]; 

  234. 	    packet[8] = leds_[2];

  235. 	    // 9, 10 flash ON, OFF times in 100ths of second?  2.5 seconds = 255

  236. 	    DBGPrintf("Joystick update Rumble/LEDs\n");

  237. 		return driver_->sendPacket(packet, 32);

  238. 	} else if (btdriver_) {

  239. 		uint8_t packet[79];

  240. 	    memset(packet, 0, sizeof(packet));

  241. //0xa2, 0x11, 0xc0, 0x20, 0xf0, 0x04, 0x00

  242. 	    packet[0] = 0x52; 

  243. 	    packet[1] = 0x11;      // Report ID

  244. 		packet[2] = 0x80;

  245. 		//packet[3] = 0x20;

  246. 		packet[4] = 0xFF;

  247. 

  248. 	    packet[7] = rumble_lValue_; // Small Rumble

  249. 	    packet[8] = rumble_rValue_; // Big rumble

  250. 	    packet[9] = leds_[0]; // RGB value 

  251. 	    packet[10] = leds_[1]; 

  252. 	    packet[11] = leds_[2];

  253. 

  254. 	    // 12, 13 flash ON, OFF times in 100ths of sedond?  2.5 seconds = 255

  255. 	    DBGPrintf("Joystick update Rumble/LEDs\n");

  256.      	btdriver_->sendL2CapCommand(packet, sizeof(packet), 0x40);

  257. 

  258.      	return true;

  259. 	}

  260. 	return false;

  261. }

  262. 

  263. static const uint8_t PS3_USER_FEEDBACK_INIT[] = {

  264.         0x00, 0x00, 0x00, 0x00, 0x00,

  265.         0x00, 0x00, 0x00, 0x00, 0x00,

  266.         0xff, 0x27, 0x10, 0x00, 0x32,

  267.         0xff, 0x27, 0x10, 0x00, 0x32,

  268.         0xff, 0x27, 0x10, 0x00, 0x32,

  269.         0xff, 0x27, 0x10, 0x00, 0x32,

  270.         0x00, 0x00, 0x00, 0x00, 0x00,

  271.         0x00, 0x00, 0x00, 0x00, 0x00,

  272.         0x00, 0x00, 0x00, 0x00, 0x00,

  273.         0x00, 0x00, 0x00 };

  274. 

  275. bool JoystickController::transmitPS3UserFeedbackMsg() {

  276. 	if (driver_) {

  277. 	    memcpy(txbuf_, PS3_USER_FEEDBACK_INIT, 48);

  278. 

  279. 	    txbuf_[1] = rumble_lValue_? rumble_timeout_ : 0;

  280. 	    txbuf_[2] = rumble_lValue_; // Small Rumble

  281. 	    txbuf_[3] = rumble_rValue_? rumble_timeout_ : 0; 

  282. 	    txbuf_[4] = rumble_rValue_; // Big rumble

  283. 	    txbuf_[9] = leds_[2] << 1; // RGB value 	// using third led now...

  284. 	    //DBGPrintf("\nJoystick update Rumble/LEDs %d %d %d %d %d\n",  txbuf_[1], txbuf_[2],  txbuf_[3],  txbuf_[4],  txbuf_[9]);

  285. 		return driver_->sendControlPacket(0x21, 9, 0x201, 0, 48, txbuf_); 

  286. 	} else if (btdriver_) {

  287. 		txbuf_[0] = 0x52;

  288. 		txbuf_[1] = 0x1;

  289. 	    memcpy(&txbuf_[2], PS3_USER_FEEDBACK_INIT, 48);

  290. 

  291. 	    txbuf_[3] = rumble_lValue_? rumble_timeout_ : 0;

  292. 	    txbuf_[4] = rumble_lValue_; // Small Rumble

  293. 	    txbuf_[5] = rumble_rValue_? rumble_timeout_ : 0; 

  294. 	    txbuf_[6] = rumble_rValue_; // Big rumble

  295. 	    txbuf_[11] = leds_[2] << 1; // RGB value 

  296. 	    DBGPrintf("\nJoystick update Rumble/LEDs %d %d %d %d %d\n",  txbuf_[3], txbuf_[4],  txbuf_[5],  txbuf_[6],  txbuf_[11]);

  297.      	btdriver_->sendL2CapCommand(txbuf_, 50, BluetoothController::CONTROL_SCID);

  298.      	return true;

  299. 	}

  300. 	return false;

  301. }

  302. 

  303. #define MOVE_REPORT_BUFFER_SIZE 7

  304. #define MOVE_HID_BUFFERSIZE 50 // Size of the buffer for the Playstation Motion Controller

  305. 

  306. bool JoystickController::transmitPS3MotionUserFeedbackMsg() {

  307. 	if (driver_) {

  308.         txbuf_[0] = 0x02; // Set report ID, this is needed for Move commands to work

  309.         txbuf_[2] = leds_[0];

  310.         txbuf_[3] = leds_[1];

  311.         txbuf_[4] = leds_[2];

  312.         txbuf_[6] = rumble_lValue_; // Set the rumble value into the write buffer

  313. 

  314. 		//return driver_->sendControlPacket(0x21, 9, 0x201, 0, MOVE_REPORT_BUFFER_SIZE, txbuf_); 

  315. 		return driver_->sendPacket(txbuf_, MOVE_REPORT_BUFFER_SIZE);

  316. 

  317. 	} else if (btdriver_) {

  318.         txbuf_[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)

  319.         txbuf_[1] = 0x02; // Report ID

  320.         txbuf_[3] = leds_[0];

  321.         txbuf_[4] = leds_[1];

  322.         txbuf_[5] = leds_[2];

  323.         txbuf_[7] = rumble_lValue_;

  324.      	btdriver_->sendL2CapCommand(txbuf_, MOVE_HID_BUFFERSIZE, BluetoothController::INTERRUPT_SCID);

  325.      	return true;

  326. 	}

  327. 	return false;

  328. }

  329. 

  330. //*****************************************************************************

  331. // Support for Joysticks that Use HID data. 

  332. //*****************************************************************************

  333. 

  334. hidclaim_t JoystickController::claim_collection(USBHIDParser *driver, Device_t *dev, uint32_t topusage)

  335. {

  336. 	// only claim Desktop/Joystick and Desktop/Gamepad

  337. 	if (topusage != 0x10004 && topusage != 0x10005 && topusage != 0x10008) return CLAIM_NO;

  338. 	// only claim from one physical device

  339. 	if (mydevice != NULL && dev != mydevice) return CLAIM_NO;

  340. 

  341. 	// Also don't allow us to claim if it is used as a standard usb object (XBox...)

  342. 	if (device != nullptr) return CLAIM_NO;

  343. 

  344. 	mydevice = dev;

  345. 	collections_claimed++;

  346. 	anychange = true; // always report values on first read

  347. 	driver_ = driver;	// remember the driver. 

  348. 	driver_->setTXBuffers(txbuf_, nullptr, sizeof(txbuf_));

  349. 	connected_ = true;		// remember that hardware is actually connected...

  350. 

  351. 	// Lets see if we know what type of joystick this is. That is, is it a PS3 or PS4 or ...

  352. 	joystickType_ = mapVIDPIDtoJoystickType(mydevice->idVendor, mydevice->idProduct, false);

  353. 	DBGPrintf("JoystickController::claim_collection joystickType_=%d\n", joystickType_);

  354. 	switch (joystickType_) {

  355. 		case PS3:

  356. 		case PS3_MOTION: // not sure yet

  357. 			additional_axis_usage_page_ = 0x1;

  358. 			additional_axis_usage_start_ = 0x100;

  359. 			additional_axis_usage_count_ = 39;

  360. 			axis_change_notify_mask_ = (uint64_t)-1;	// Start off assume all bits 

  361. 			break;

  362. 		case PS4:

  363. 			additional_axis_usage_page_ = 0xFF00;

  364. 			additional_axis_usage_start_ = 0x21;

  365. 			additional_axis_usage_count_ = 54;

  366. 			axis_change_notify_mask_ = (uint64_t)0xfffffffffffff3ffl;	// Start off assume all bits - 10 and 11

  367. 			break;

  368. 		default: 

  369. 			additional_axis_usage_page_ = 0x09;

  370. 			additional_axis_usage_start_ = 0x21;

  371. 			additional_axis_usage_count_ = 5;

  372. 			axis_change_notify_mask_ = 0x3ff;	// Start off assume only the 10 bits...

  373. 	}

  374. 	DBGPrintf("Claim Additional axis: %x %x %d\n", additional_axis_usage_page_, additional_axis_usage_start_, additional_axis_usage_count_);

  375. 	return CLAIM_REPORT;

  376. }

  377. 

  378. void JoystickController::disconnect_collection(Device_t *dev)

  379. {

  380. 	if (--collections_claimed == 0) {

  381. 		mydevice = NULL;

  382. 		driver_ = nullptr;

  383. 		axis_mask_ = 0;	

  384. 		axis_changed_mask_ = 0;

  385. 	}

  386. }

  387. 

  388. void JoystickController::hid_input_begin(uint32_t topusage, uint32_t type, int lgmin, int lgmax)

  389. {

  390. 	// TODO: set up translation from logical min/max to consistent 16 bit scale

  391. }

  392. 

  393. void JoystickController::hid_input_data(uint32_t usage, int32_t value)

  394. {

  395. 	DBGPrintf("joystickType_=%d\n", joystickType_);

  396. 	DBGPrintf("Joystick: usage=%X, value=%d\n", usage, value);

  397. 	uint32_t usage_page = usage >> 16;

  398. 	usage &= 0xFFFF;

  399. 	if (usage_page == 9 && usage >= 1 && usage <= 32) {

  400. 		uint32_t bit = 1 << (usage -1);

  401. 		if (value == 0) {

  402. 			if (buttons & bit) {

  403. 				buttons &= ~bit;

  404. 				anychange = true;

  405. 			}

  406. 		} else {

  407. 			if (!(buttons & bit)) {

  408. 				buttons |= bit;

  409. 				anychange = true;

  410. 			}

  411. 		}

  412. 	} else if (usage_page == 1 && usage >= 0x30 && usage <= 0x39) {

  413. 		// TODO: need scaling of value to consistent API, 16 bit signed?

  414. 		// TODO: many joysticks repeat slider usage.  Detect & map to axis?

  415. 		uint32_t i = usage - 0x30;

  416. 		axis_mask_ |= (1 << i);		// Keep record of which axis we have data on.

  417. 		if (axis[i] != value) {

  418. 			axis[i] = value;

  419. 			axis_changed_mask_ |= (1 << i);

  420. 			if (axis_changed_mask_ & axis_change_notify_mask_)

  421. 				anychange = true;

  422. 		}

  423. 	} else if (usage_page == additional_axis_usage_page_) {

  424. 		// see if the usage is witin range.

  425. 		//DBGPrintf("UP: usage_page=%x usage=%x User: %x %d\n", usage_page, usage, user_buttons_usage_start, user_buttons_count_);

  426. 		if ((usage >= additional_axis_usage_start_) && (usage < (additional_axis_usage_start_ + additional_axis_usage_count_))) {

  427. 			// We are in the user range. 

  428. 			uint16_t usage_index = usage - additional_axis_usage_start_ + STANDARD_AXIS_COUNT;

  429. 			if (usage_index < (sizeof(axis)/sizeof(axis[0]))) {

  430. 				if (axis[usage_index] != value) {

  431. 					axis[usage_index] = value;

  432. 					if (usage_index > 63) usage_index = 63;	// don't overflow our mask

  433. 					axis_changed_mask_ |= ((uint64_t)1 << usage_index);		// Keep track of which ones changed.

  434. 					if (axis_changed_mask_ & axis_change_notify_mask_)

  435. 						anychange = true;	// We have changes... 

  436. 				}

  437. 				axis_mask_ |= ((uint64_t)1 << usage_index);		// Keep record of which axis we have data on.

  438. 			}

  439. 			//DBGPrintf("UB: index=%x value=%x\n", usage_index, value);

  440. 		}

  441. 

  442. 	} else {

  443. 		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_);

  444. 

  445. 	}

  446. 	// TODO: hat switch?

  447. }

  448. 

  449. void JoystickController::hid_input_end()

  450. {

  451. 	if (anychange) {

  452. 		joystickEvent = true;

  453. 	}

  454. }

  455. 

  456. bool JoystickController::hid_process_out_data(const Transfer_t *transfer) 

  457. {

  458. 	//DBGPrintf("JoystickController::hid_process_out_data\n");

  459. 	return true;

  460. }

  461. 

  462. void JoystickController::joystickDataClear() {

  463. 	joystickEvent = false;

  464. 	anychange = false;

  465. 	axis_changed_mask_ = 0;

  466. 	axis_mask_ = 0;

  467. }

  468. 

  469. //*****************************************************************************

  470. // Support for Joysticks that are class specific and do not use HID

  471. // Example: XBox One controller. 

  472. //*****************************************************************************

  473. 

  474. static  uint8_t xboxone_start_input[] = {0x05, 0x20, 0x00, 0x01, 0x00};

  475. static  uint8_t xbox360w_inquire_present[] = {0x08, 0x00, 0x0F, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

  476. 

  477. bool JoystickController::claim(Device_t *dev, int type, const uint8_t *descriptors, uint32_t len)

  478. {

  479. 	println("JoystickController claim this=", (uint32_t)this, HEX);

  480. 

  481. 	// Don't try to claim if it is used as USB device or HID device

  482. 	if (mydevice != NULL) return false;

  483. 	if (device != nullptr) return false;

  484. 

  485. 	// Try claiming at the interface level.

  486. 	if (type != 1) return false;

  487. 	print_hexbytes(descriptors, len);

  488. 

  489. 	JoystickController::joytype_t jtype = mapVIDPIDtoJoystickType(dev->idVendor, dev->idProduct, true);

  490. 	println("Jtype=", (uint8_t)jtype, DEC);

  491. 	if (jtype == UNKNOWN)

  492. 		return false; 

  493. 

  494. 	// XBOX One

  495. 	//  0  1  2  3  4  5  6  7  8 *9 10  1  2  3  4  5 *6  7  8  9 20  1  2  3  4  5  6  7  8  9 30  1... 

  496. 	// 09 04 00 00 02 FF 47 D0 00 07 05 02 03 40 00 04 07 05 82 03 40 00 04 09 04 01 00 00 FF 47 D0 00 

  497. 	// Lets do some verifications to make sure. 

  498. 

  499. 	// XBOX 360 wireless... Has 8 interfaces.  4 joysticks (1, 3, 5, 7) and 4 headphones assume 2,4,6, 8... 

  500. 	// Shows data for #1 only... 

  501. 	// Also they have some unknown data type we need to ignore between interface and end points.

  502. 	//  0  1  2  3  4  5  6  7  8 *9 10  1  2  3  4  5 *6  7  8  9 20  1  2  3  4  5  6  7  8 

  503. 	// 09 04 00 00 02 FF 5D 81 00 14 22 00 01 13 81 1D 00 17 01 02 08 13 01 0C 00 0C 01 02 08 

  504. 

  505.   	// 29 30  1  2  3  4  5  6  7  8  9 40 41 42

  506. 	// 07 05 81 03 20 00 01 07 05 01 03 20 00 08 

  507. 

  508. 	// Switch

  509. 	// 09 04 00 00 02 FF 5D 01 00 

  510. 	// 10 21 10 01 01 24 81 14 03 00 03 13 02 00 03 00 

  511. 	// 07 05 81 03 20 00 08 

  512. 	// 07 05 02 03 20 00 08 

  513. 

  514. 

  515. 

  516. 	if (len < 9+7+7) return false;

  517. 

  518. 	// Some common stuff for both XBoxs

  519. 	uint32_t count_end_points = descriptors[4];

  520. 	if (count_end_points < 2) return false;

  521. 	if (descriptors[5] != 0xff) return false; // bInterfaceClass, 3 = HID

  522. 	rx_ep_ = 0;

  523. 	uint32_t txep = 0;

  524. 	uint8_t rx_interval = 0;

  525. 	uint8_t tx_interval = 0;

  526. 	rx_size_ = 0;

  527. 	tx_size_ = 0;

  528. 	uint32_t descriptor_index = 9; 

  529. 	if (descriptors[descriptor_index+1] == 0x22)  {

  530. 		if (descriptors[descriptor_index] != 0x14) return false; // only support specific versions...

  531. 		descriptor_index += descriptors[descriptor_index]; // XBox360w ignore this unknown setup...

  532. 	}	

  533. 	while ((rx_ep_ == 0) || txep == 0) {

  534. 		print("  Index:", descriptor_index, DEC);

  535. 

  536. 		if (descriptor_index >= len) return false;  		// we ran off the end and did not get end points

  537. 		// see if the next data is an end point descript

  538. 		if ((descriptors[descriptor_index] == 7) && (descriptors[descriptor_index+1] == 5)) {

  539. 			if ((descriptors[descriptor_index+3] == 3) 				// Type 3...

  540. 				&& (descriptors[descriptor_index+4] <= 64)

  541. 				&& (descriptors[descriptor_index+5] == 0)) {

  542. 				// have a bulk EP size 

  543. 				if (descriptors[descriptor_index+2] & 0x80 ) {

  544. 					rx_ep_ = descriptors[descriptor_index+2];

  545. 					rx_size_ = descriptors[descriptor_index+4];

  546. 					rx_interval = descriptors[descriptor_index+6];

  547. 				} else {

  548. 					txep = descriptors[descriptor_index+2]; 

  549. 					tx_size_ = descriptors[descriptor_index+4];

  550. 					tx_interval = descriptors[descriptor_index+6];

  551. 				}

  552. 			}

  553. 		}

  554. 		descriptor_index += descriptors[descriptor_index];  // setup to look at next one...

  555. 	}

  556. 	if ((rx_ep_ == 0) || (txep == 0)) return false; // did not find two end points.

  557. 	print("JoystickController, rx_ep_=", rx_ep_ & 15);

  558. 	print("(", rx_size_);

  559. 	print("), txep=", txep);

  560. 	print("(", tx_size_);

  561. 	println(")");

  562. 	rxpipe_ = new_Pipe(dev, 3, rx_ep_ & 15, 1, rx_size_, rx_interval);

  563. 	if (!rxpipe_) return false;

  564. 	txpipe_ = new_Pipe(dev, 3, txep, 0, tx_size_, tx_interval);

  565. 	if (!txpipe_) {

  566. 		//free_Pipe(rxpipe_);

  567. 		return false;

  568. 	}

  569. 	rxpipe_->callback_function = rx_callback;

  570. 	queue_Data_Transfer(rxpipe_, rxbuf_, rx_size_, this);

  571. 

  572. 	txpipe_->callback_function = tx_callback;

  573. 

  574. 	if (jtype == XBOXONE) {

  575. 		queue_Data_Transfer(txpipe_, xboxone_start_input, sizeof(xboxone_start_input), this);

  576. 		connected_ = true;		// remember that hardware is actually connected...

  577. 	} else if (jtype == XBOX360) {

  578. 		queue_Data_Transfer(txpipe_, xbox360w_inquire_present, sizeof(xbox360w_inquire_present), this);

  579. 		connected_ = 0;		// remember that hardware is actually connected...

  580. 	}

  581. 	memset(axis, 0, sizeof(axis));	// clear out any data. 

  582. 	joystickType_ = jtype;		// remember we are an XBox One. 

  583. 	DBGPrintf("   JoystickController::claim joystickType_ %d\n", joystickType_);

  584. 	return true;

  585. }

  586. 

  587. void JoystickController::control(const Transfer_t *transfer)

  588. {

  589. }

  590. 

  591. 

  592. /************************************************************/

  593. //  Interrupt-based Data Movement

  594. /************************************************************/

  595. 

  596. void JoystickController::rx_callback(const Transfer_t *transfer)

  597. {

  598. 	if (!transfer->driver) return;

  599. 	((JoystickController *)(transfer->driver))->rx_data(transfer);

  600. }

  601. 

  602. void JoystickController::tx_callback(const Transfer_t *transfer)

  603. {

  604. 	if (!transfer->driver) return;

  605. 	((JoystickController *)(transfer->driver))->tx_data(transfer);

  606. }

  607. 

  608. 

  609. 

  610. /************************************************************/

  611. //  Interrupt-based Data Movement

  612. // XBox one input data when type == 0x20

  613. // Information came from several places on the web including: 

  614. // https://github.com/quantus/xbox-one-controller-protocol

  615. /************************************************************/

  616. // 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

  617. // 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

  618. typedef struct {

  619. 	uint8_t type;

  620. 	uint8_t const_0;

  621. 	uint16_t id;

  622. 	// From online references button order: 

  623. 	//     sync, dummy, start, back, a, b, x, y

  624. 	//     dpad up, down left, right

  625. 	//	   lb, rb, left stick, right stick

  626. 	// Axis: 

  627. 	//     lt, rt, lx, ly, rx, ry

  628. 	//     

  629. 	uint16_t buttons; 

  630. 	int16_t	axis[6];

  631. } xbox1data20_t;

  632. 

  633. typedef struct {

  634. 	uint8_t state;

  635. 	uint8_t id_or_type;

  636. 	uint16_t controller_status;

  637. 	uint16_t unknown;

  638. 	// From online references button order: 

  639. 	//     sync, dummy, start, back, a, b, x, y

  640. 	//     dpad up, down left, right

  641. 	//	   lb, rb, left stick, right stick

  642. 	// Axis: 

  643. 	//     lt, rt, lx, ly, rx, ry

  644. 	//

  645. 	uint16_t buttons; 

  646. 	uint8_t lt;

  647. 	uint8_t rt;

  648. 	int16_t	axis[4];

  649. } xbox360data_t;

  650. 

  651. typedef struct {

  652. 	uint8_t state;

  653. 	uint8_t id_or_type;

  654. 	// From online references button order: 

  655. 	//     sync, dummy, start, back, a, b, x, y

  656. 	//     dpad up, down left, right

  657. 	//	   lb, rb, left stick, right stick

  658. 	// Axis: 

  659. 	//     lt, rt, lx, ly, rx, ry

  660. 	//

  661. 	uint16_t buttons; 

  662. 	uint8_t lt;

  663. 	uint8_t rt;

  664. 	int16_t	axis[4];

  665. } switchdataUSB_t;

  666. 

  667. static const uint8_t xbox_axis_order_mapping[] = {3, 4, 0, 1, 2, 5};

  668. 

  669. void JoystickController::rx_data(const Transfer_t *transfer)

  670. {

  671. 	#ifdef  DEBUG_JOYSTICK

  672. 	print("JoystickController::rx_data (", joystickType_, DEC);

  673. 	print("): ");

  674. 	print_hexbytes((uint8_t*)transfer->buffer, transfer->length);

  675. 	#endif

  676. 

  677. 	if (joystickType_ == XBOXONE) {

  678. 		// Process XBOX One data

  679. 		axis_mask_ = 0x3f;	

  680. 		axis_changed_mask_ = 0;	// assume none for now

  681. 		xbox1data20_t *xb1d = (xbox1data20_t *)transfer->buffer;

  682. 		if ((xb1d->type == 0x20) && (transfer->length >= sizeof (xbox1data20_t))) {

  683. 			// We have a data transfer.  Lets see what is new...

  684. 			if (xb1d->buttons != buttons) {

  685. 				buttons = xb1d->buttons;

  686. 				anychange = true;

  687. 				joystickEvent = true;

  688. 				println("  Button Change: ", buttons, HEX);

  689. 			}

  690. 			for (uint8_t i = 0; i < sizeof (xbox_axis_order_mapping); i++) {

  691. 				// The first two values were unsigned. 

  692. 				int axis_value = (i < 2)? (int)(uint16_t)xb1d->axis[i] : xb1d->axis[i];

  693. 				if (axis_value != axis[xbox_axis_order_mapping[i]]) {

  694. 					axis[xbox_axis_order_mapping[i]] = axis_value;

  695. 					axis_changed_mask_ |= (1 << xbox_axis_order_mapping[i]);

  696. 					anychange = true;

  697. 				}

  698. 			}

  699. 			joystickEvent = true;

  700. 		}

  701. 

  702. 	} else if (joystickType_ == XBOX360) {

  703. 		// First byte appears to status - if the byte is 0x8 it is a connect or disconnect of the controller. 

  704. 		xbox360data_t  *xb360d = (xbox360data_t *)transfer->buffer;

  705. 		if (xb360d->state == 0x08) {

  706. 			if (xb360d->id_or_type != connected_) {

  707. 				connected_ = xb360d->id_or_type;	// remember it... 

  708. 				if (connected_) {

  709. 					println("XBox360w - Connected type:", connected_, HEX);

  710. 					// rx_ep_ should be 1, 3, 5, 7 for the wireless convert to 2-5 on led

  711. 					setLEDs(2+rx_ep_/2);	// Right now hard coded to first joystick...

  712. 

  713. 				} else {

  714. 					println("XBox360w - disconnected");

  715. 				}

  716. 			}

  717. 		} else if((xb360d->id_or_type == 0x00) && (xb360d->controller_status & 0x1300)) {

  718. 			  // Controller status report - Maybe we should save away and allow the user access?

  719. 	            println("XBox360w - controllerStatus: ", xb360d->controller_status, HEX);

  720.         } else if(xb360d->id_or_type == 0x01) { // Lets only process report 1.

  721. 			//const uint8_t *pbuffer = (uint8_t*)transfer->buffer;

  722.         	//for (uint8_t i = 0; i < transfer->length; i++) DBGPrintf("%02x ", pbuffer[i]);

  723.         	//DBGPrintf("\n");

  724. 	        

  725. 	        if (buttons != xb360d->buttons) {

  726. 	        	buttons = xb360d->buttons;

  727. 	        	anychange = true;

  728. 	        }

  729. 			axis_mask_ = 0x3f;	

  730. 			axis_changed_mask_ = 0;	// assume none for now

  731. 

  732. 			for (uint8_t i = 0; i < 4; i++) {

  733. 				if (axis[i] != xb360d->axis[i]) {

  734. 					axis[i] = xb360d->axis[i];

  735. 					axis_changed_mask_ |= (1 << i);

  736. 					anychange = true;

  737. 				}

  738. 			}

  739. 			// the two triggers show up as 4 and 5

  740. 			if (axis[4] != xb360d->lt) {

  741. 				axis[4] = xb360d->lt;

  742. 				axis_changed_mask_ |= (1 << 4);

  743. 				anychange = true;

  744. 			}

  745. 

  746. 			if (axis[5] != xb360d->rt) {

  747. 				axis[5] = xb360d->rt;

  748. 				axis_changed_mask_ |= (1 << 5);

  749. 				anychange = true;

  750. 			}

  751. 

  752. 			if (anychange) joystickEvent = true;

  753. 		}

  754. 	} else if (joystickType_ == SWITCH) {

  755. 		switchdataUSB_t  *switchd = (switchdataUSB_t *)transfer->buffer;

  756.         if (buttons != switchd->buttons) {

  757.         	buttons = switchd->buttons;

  758.         	anychange = true;

  759.         }

  760. 		axis_mask_ = 0x3f;	

  761. 		axis_changed_mask_ = 0;	// assume none for now

  762. 

  763. 		for (uint8_t i = 0; i < 4; i++) {

  764. 			if (axis[i] != switchd->axis[i]) {

  765. 				axis[i] = switchd->axis[i];

  766. 				axis_changed_mask_ |= (1 << i);

  767. 				anychange = true;

  768. 			}

  769. 		}

  770. 		// the two triggers show up as 4 and 5

  771. 		if (axis[4] != switchd->lt) {

  772. 			axis[4] = switchd->lt;

  773. 			axis_changed_mask_ |= (1 << 4);

  774. 			anychange = true;

  775. 		}

  776. 

  777. 		if (axis[5] != switchd->rt) {

  778. 			axis[5] = switchd->rt;

  779. 			axis_changed_mask_ |= (1 << 5);

  780. 			anychange = true;

  781. 		}

  782. 

  783. 		if (anychange) joystickEvent = true;

  784. 	}

  785. 

  786. 	queue_Data_Transfer(rxpipe_, rxbuf_, rx_size_, this);

  787. }

  788. 

  789. void JoystickController::tx_data(const Transfer_t *transfer)

  790. {

  791. }

  792. 

  793. void JoystickController::disconnect()

  794. {

  795. 	axis_mask_ = 0;	

  796. 	axis_changed_mask_ = 0;

  797. 	// TODO: free resources

  798. }

  799. 

  800. bool JoystickController::claim_bluetooth(BluetoothController *driver, uint32_t bluetooth_class, uint8_t *remoteName) 

  801. {

  802. 	// If we are already in use than don't grab another one.  Likewise don't grab if it is used as USB or HID object

  803. 	if (btdevice && (btdevice != (Device_t*)driver)) return false;

  804. 	if (mydevice != NULL) return false;

  805. 	if (device != nullptr) return false;

  806. 

  807. 	if ((((bluetooth_class & 0xff00) == 0x2500) || (((bluetooth_class & 0xff00) == 0x500))) && ((bluetooth_class & 0x3C) == 0x08)) {

  808. 		DBGPrintf("JoystickController::claim_bluetooth TRUE\n");

  809. 		btdriver_ = driver;

  810. 		btdevice = (Device_t*)driver;	// remember this way 

  811. 		if (remoteName) mapNameToJoystickType(remoteName);

  812. 		return true;

  813. 	}

  814. 

  815. 	if (remoteName && mapNameToJoystickType(remoteName)) {

  816. 		if ((joystickType_ == PS3) || (joystickType_ == PS3_MOTION)) {

  817. 			DBGPrintf("JoystickController::claim_bluetooth TRUE PS3 hack...\n");

  818. 			btdriver_ = driver;

  819. 			btdevice = (Device_t*)driver;	// remember this way 

  820. 			special_process_required = SP_PS3_IDS; 		// PS3 maybe needs different IDS. 

  821. 			return true;

  822. 		}

  823. 	}

  824. 	return false;

  825. }

  826. 

  827. 

  828. bool JoystickController::process_bluetooth_HID_data(const uint8_t *data, uint16_t length) 

  829. {

  830. 	// Example data from PS4 controller

  831. 	//01 7e 7f 82 84 08 00 00 00 00

  832. 	//   LX LY RX RY BT BT PS LT RT

  833. 	DBGPrintf("JoystickController::process_bluetooth_HID_data: data[0]=%x\n", data[0]);

  834. 	// May have to look at this one with other controllers...

  835. 	if (data[0] == 1) {

  836. 		//print("  Joystick Data: ");

  837. 		// print_hexbytes(data, length);

  838. 		if (length > TOTAL_AXIS_COUNT) length = TOTAL_AXIS_COUNT;	// don't overflow arrays...

  839. 		DBGPrintf("  Joystick Data: ");

  840. 		for(uint16_t i =0; i < length; i++) DBGPrintf("%02x ", data[i]);

  841. 		DBGPrintf("\r\n");	

  842. 		if (joystickType_ == PS3) {

  843. 			// Quick and dirty hack to match PS3 HID data

  844. 			uint32_t cur_buttons = data[2] | ((uint16_t)data[3] << 8) | ((uint32_t)data[4] << 16); 

  845. 			if (cur_buttons != buttons) {

  846. 				buttons = cur_buttons;

  847. 				joystickEvent = true;	// something changed.

  848. 			}

  849. 

  850. 			uint64_t mask = 0x1;

  851. 			axis_mask_ = 0x27;	// assume bits 0, 1, 2, 5

  852. 			for (uint16_t i = 0; i < 3; i++) {

  853. 				if (axis[i] != data[i+6]) {

  854. 					axis_changed_mask_ |= mask;

  855. 					axis[i] = data[i+6];

  856. 				}

  857. 				mask <<= 1;	// shift down the mask.

  858. 			}

  859. 			if (axis[5] != data[9]) {

  860. 				axis_changed_mask_ |= (1<<5);

  861. 				axis[5] = data[9];

  862. 			}

  863. 			

  864. 			if (axis[3] != data[18]) {

  865. 				axis_changed_mask_ |= (1<<3);

  866. 				axis[3] = data[18];

  867. 			}

  868. 			

  869. 			if (axis[4] != data[19]) {

  870. 				axis_changed_mask_ |= (1<<4);

  871. 				axis[4] = data[19];

  872. 			}

  873. 			

  874. 			// Then rest of data

  875. 			mask = 0x1 << 10;	// setup for other bits

  876. 			for (uint16_t i = 10; i < length; i++ ) {

  877. 				axis_mask_ |= mask;

  878. 				if(data[i] != axis[i]) { 

  879. 					axis_changed_mask_ |= mask;

  880. 					axis[i] = data[i];

  881. 				} 

  882. 				mask <<= 1;	// shift down the mask.

  883. 			}

  884. 		} else if (joystickType_ == PS3_MOTION) {

  885. 			// Quick and dirty PS3_Motion data.

  886. 			uint32_t cur_buttons = data[1] | ((uint16_t)data[2] << 8) | ((uint32_t)data[3] << 16); 

  887. 			if (cur_buttons != buttons) {

  888. 				buttons = cur_buttons;

  889. 				joystickEvent = true;	// something changed.

  890. 			}

  891. 

  892. 			// Hard to know what is best here. for now just copy raw data over... 

  893. 			// will do this for now... Format of thought to be data.

  894. 			//  data[1-3] Buttons (mentioned 4 as well but appears to be counter

  895. 			// axis[0-1] data[5] Trigger, Previous trigger value

  896. 			// 2-5 Unknown probably place holders for Axis like data for other PS3

  897. 			// 6 - Time stamp

  898. 			// 7 - Battery

  899. 			// 8-19 - Accel: XL, XH, YL, YH, ZL, ZH, XL2, XH2, YL2, YH2, ZL2, ZH2

  900. 			// 20-31 - Gyro: Xl,Xh,Yl,Yh,Zl,Zh,Xl2,Xh2,Yl2,Yh2,Zl2,Zh2

  901. 			// 32 - Temp High

  902. 			// 33 - Temp Low (4 bits)  Maybe Magneto x High on other?? 

  903. 			uint64_t mask = 0x1;

  904. 			axis_mask_ = 0;	// assume bits 0, 1, 2, 5

  905. 			// Then rest of data

  906. 			mask = 0x1 << 10;	// setup for other bits

  907. 			for (uint16_t i = 5; i < length; i++ ) {

  908. 				axis_mask_ |= mask;

  909. 				if(data[i] != axis[i-5]) { 

  910. 					axis_changed_mask_ |= mask;

  911. 					axis[i-5] = data[i];

  912. 				} 

  913. 				mask <<= 1;	// shift down the mask.

  914. 			}

  915. 

  916. 		} else {

  917. 			uint64_t mask = 0x1;

  918. 			axis_mask_ = 0;

  919. 

  920. 			for (uint16_t i = 0; i < length; i++ ) {

  921. 				axis_mask_ |= mask;

  922. 				if(data[i] != axis[i]) { 

  923. 					axis_changed_mask_ |= mask;

  924. 					axis[i] = data[i];

  925. 				} 

  926. 				mask <<= 1;	// shift down the mask.

  927. //				DBGPrintf("%02x ", axis[i]);

  928. 			}

  929. 

  930. 		}

  931. 

  932. 		if (axis_changed_mask_ & axis_change_notify_mask_)

  933. 			joystickEvent = true;

  934. 		connected_ = true;

  935. 		return true;

  936. 

  937. 	} else if(data[0] == 0x11){

  938. 		DBGPrintf("\n  Joystick Data: ");

  939. 		uint64_t mask = 0x1;

  940. 		axis_mask_ = 0;

  941. 		axis_changed_mask_ = 0;

  942. 		

  943. 		//This moves data to be equivalent to what we see for

  944. 		//data[0] = 0x01

  945. 		uint8_t tmp_data[length-2];

  946. 		

  947. 		for (uint16_t i = 0; i < (length-2); i++ ) {

  948. 			tmp_data[i] = 0;

  949. 			tmp_data[i] = data[i+2];

  950. 		}

  951. 		

  952. 		/*

  953. 		 * [1] LX, [2] = LY, [3] = RX, [4] = RY

  954. 		 * [5] combo, tri, cir, x, sqr, D-PAD (4bits, 0-3

  955. 		 * [6] R3,L3, opt, share, R2, L2, R1, L1

  956. 		 * [7] Counter (bit7-2), T-PAD, PS

  957. 		 * [8] Left Trigger, [9] Right Trigger

  958. 		 * [10-11] Timestamp

  959. 		 * [12] Battery (0 to 0xff)

  960. 		 * [13-14] acceleration x

  961. 		 * [15-16] acceleration y

  962. 		 * [17-18] acceleration z

  963. 		 * [19-20] gyro x

  964. 		 * [21-22] gyro y

  965. 		 * [23-24] gyro z

  966. 		 * [25-29] unknown

  967. 		 * [30] 0x00,phone,mic, usb, battery level (4bits)

  968. 		 * rest is trackpad?  to do implement?

  969. 		 */

  970. 		//PS Bit

  971. 		tmp_data[7] = (tmp_data[7] >> 0) & 1;

  972. 		//set arrow buttons to axis[0]

  973. 		tmp_data[10] = tmp_data[5] & ((1 << 4) - 1);

  974. 		//set buttons for last 4bits in the axis[5]

  975. 		tmp_data[5] = tmp_data[5] >> 4;

  976. 		

  977. 	

  978. 		// Quick and dirty hack to match PS4 HID data

  979. 		uint32_t cur_buttons = tmp_data[7] | (tmp_data[10]) | ((tmp_data[6]*10)) | ((uint16_t)tmp_data[5] << 16) ; 

  980. 		if (cur_buttons != buttons) {

  981. 			buttons = cur_buttons;

  982. 			joystickEvent = true;	// something changed.

  983. 		}

  984. 		

  985. 		mask = 0x1;

  986. 		axis_mask_ = 0x27;	// assume bits 0, 1, 2, 5

  987. 		for (uint16_t i = 0; i < 3; i++) {

  988. 			if (axis[i] != tmp_data[i+1]) {

  989. 				axis_changed_mask_ |= mask;

  990. 				axis[i] = tmp_data[i+1];

  991. 			}

  992. 			mask <<= 1;	// shift down the mask.

  993. 		}

  994. 		if (axis[5] != tmp_data[4]) {

  995. 			axis_changed_mask_ |= (1<<5);

  996. 			axis[5] = tmp_data[4];

  997. 		}

  998. 		

  999. 		if (axis[3] != tmp_data[8]) {

  1000. 			axis_changed_mask_ |= (1<<3);

  1001. 			axis[3] = tmp_data[8];

  1002. 		}

  1003. 		

  1004. 		if (axis[4] != tmp_data[9]) {

  1005. 			axis_changed_mask_ |= (1<<4);

  1006. 			axis[4] = tmp_data[9];

  1007. 		}

  1008. 		

  1009. 		//limit for masking

  1010. 		mask = 0x1;

  1011. 		for (uint16_t i = 6; i < (64); i++ ) {

  1012. 			axis_mask_ |= mask;

  1013. 			if(tmp_data[i] != axis[i]) { 

  1014. 				axis_changed_mask_ |= mask;

  1015. 				axis[i] = tmp_data[i];

  1016. 			}

  1017. 			mask <<= 1;	// shift down the mask.

  1018. 			DBGPrintf("%02x ", axis[i]);

  1019. 		}

  1020. 		DBGPrintf("\n");

  1021. 		//DBGPrintf("Axis Mask (axis_mask_, axis_changed_mask_; %d, %d\n", axis_mask_,axis_changed_mask_);

  1022. 		joystickEvent = true;

  1023. 		connected_ = true;

  1024. 	}

  1025. 	return false;

  1026. }

  1027. 

  1028. bool JoystickController::mapNameToJoystickType(const uint8_t *remoteName)

  1029. {

  1030. 	// Sort of a hack, but try to map the name given from remote to a type...

  1031. 	if (strncmp((const char *)remoteName, "Wireless Controller", 19) == 0) {

  1032. 		DBGPrintf("  JoystickController::mapNameToJoystickType %s - set to PS4\n", remoteName);

  1033. 		joystickType_ = PS4;

  1034. 	} else if (strncmp((const char *)remoteName, "PLAYSTATION(R)3", 15) == 0) {

  1035. 		DBGPrintf("  JoystickController::mapNameToJoystickType %x %s - set to PS3\n", (uint32_t)this, remoteName);

  1036. 		joystickType_ = PS3;

  1037. 	} else if (strncmp((const char *)remoteName, "Navigation Controller", 21) == 0) {

  1038. 		DBGPrintf("  JoystickController::mapNameToJoystickType %x %s - set to PS3\n", (uint32_t)this, remoteName);

  1039. 		joystickType_ = PS3;

  1040. 	} else if (strncmp((const char *)remoteName, "Motion Controller", 17) == 0) {

  1041. 		DBGPrintf("  JoystickController::mapNameToJoystickType %x %s - set to PS3 Motion\n", (uint32_t)this, remoteName);

  1042. 		joystickType_ = PS3_MOTION;

  1043. 	} else if (strncmp((const char *)remoteName, "Xbox Wireless", 13) == 0) {

  1044. 		DBGPrintf("  JoystickController::mapNameToJoystickType %x %s - set to XBOXONE\n", (uint32_t)this, remoteName);

  1045. 		joystickType_ = XBOXONE;

  1046. 	} else {

  1047. 		DBGPrintf("  JoystickController::mapNameToJoystickType %s - Unknown\n", remoteName);

  1048. 	}

  1049. 	DBGPrintf("  Joystick Type: %d\n", joystickType_);

  1050. 	return true;

  1051. }

  1052. 

  1053. 

  1054. bool JoystickController::remoteNameComplete(const uint8_t *remoteName) 

  1055. {

  1056. 	// Sort of a hack, but try to map the name given from remote to a type...

  1057. 	if (mapNameToJoystickType(remoteName)) {

  1058. 		switch (joystickType_) {

  1059. 			case PS4: special_process_required = SP_NEED_CONNECT; break;

  1060. 			case PS3: special_process_required = SP_PS3_IDS; break;

  1061. 			case PS3_MOTION: special_process_required = SP_PS3_IDS; break;

  1062. 			default: 

  1063. 				break;

  1064. 		}

  1065. 	}

  1066. 	return true;

  1067. }

  1068. 

  1069. void JoystickController::connectionComplete() 

  1070. {

  1071. 	DBGPrintf("  JoystickController::connectionComplete %x joystick type %d\n", (uint32_t)this, joystickType_);

  1072. 	switch (joystickType_) {

  1073. 	case PS4:

  1074. 		{

  1075. 			uint8_t packet[2];

  1076. 			packet[0] = 0x43; // HID BT Get_report (0x40) | Report Type (Feature 0x03)

  1077. 			packet[1] = 0x02; // Report ID

  1078. 			DBGPrintf("Set PS4 report\n");

  1079. 			delay(1);

  1080. 			btdriver_->sendL2CapCommand(packet, sizeof(packet), 0x40);			

  1081. 		}

  1082. 		break;

  1083. 	case PS3:

  1084. 		{

  1085. 			uint8_t packet[6];

  1086. 			packet[0] = 0x53; // HID BT Set_report (0x50) | Report Type (Feature 0x03)

  1087. 			packet[1] = 0xF4; // Report ID

  1088. 			packet[2] = 0x42; // Special PS3 Controller enable commands

  1089. 			packet[3] = 0x03;

  1090. 			packet[4] = 0x00;

  1091. 			packet[5] = 0x00;

  1092. 

  1093. 			DBGPrintf("enable six axis\n");

  1094. 			delay(1);

  1095. 			btdriver_->sendL2CapCommand(packet, sizeof(packet), BluetoothController::CONTROL_SCID);

  1096. 		}

  1097. 		break;

  1098. 	case PS3_MOTION:	

  1099. 		setLEDs(0, 0xff, 0);	// Maybe try setting to green? 

  1100. 	default:

  1101. 		break;	

  1102. 	}

  1103. }

  1104. 

  1105. void JoystickController::release_bluetooth() 

  1106. {

  1107. 	btdevice = nullptr;	// remember this way 

  1108. 	btdriver_ = nullptr;

  1109. 	connected_ = false;

  1110. 	special_process_required = false;

  1111. 

  1112. }

  1113. 

  1114. 

  1115. bool JoystickController::PS3Pair(uint8_t* bdaddr) {

  1116. 	if (!driver_) return false;

  1117.  	if (joystickType_ == PS3) {

  1118.  	    /* Set the internal Bluetooth address */

  1119. 	    txbuf_[0] = 0x01;

  1120. 	    txbuf_[1] = 0x00;

  1121. 

  1122. 	    for(uint8_t i = 0; i < 6; i++)

  1123. 	            txbuf_[i + 2] = bdaddr[5 - i]; // Copy into buffer, has to be written reversed, so it is MSB first

  1124. 

  1125. 	    // 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

  1126. 		return driver_->sendControlPacket(0x21, 9, 0x3f5, 0, 8, txbuf_); 

  1127. 	} else if (joystickType_ == PS3_MOTION) {

  1128. 		// Slightly different than other PS3 units...

  1129. 	    txbuf_[0] = 0x05;

  1130. 	    for(uint8_t i = 0; i < 6; i++)

  1131. 	            txbuf_[i + 1] = bdaddr[i]; // Order different looks like LSB First?

  1132. 

  1133.         txbuf_[7] = 0x10;

  1134.         txbuf_[8] = 0x01;

  1135.         txbuf_[9] = 0x02;

  1136.         txbuf_[10] = 0x12;

  1137. 	    // 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

  1138. 		return driver_->sendControlPacket(0x21, 9, 0x305, 0, 11, txbuf_); 

  1139. 	}

  1140. 	return false;

  1141. }