visus
/
framework-arduinoteensy-cxx20
Beobachten 1
Favorisieren 0
Fork 0
Code Issues 0 Pull-Requests 0 Releases 0 Wiki Aktivität
PlatformIO package of the Teensy core framework compatible with GCC 10 & C++20
Du kannst nicht mehr als 25 Themen auswählen Themen müssen entweder mit einem Buchstaben oder einer Ziffer beginnen. Sie können Bindestriche („-“) enthalten und bis zu 35 Zeichen lang sein.
3 Commits
1 Branch
Struktur: 8b2d08b377
Branches Tags
${ item.name }
Erstelle Branch ${ searchTerm }
von „8b2d08b377“
${ noResults }
framework-arduinoteensy-cxx20/libraries/USBHost_t36/joystick.cpp

1076 Zeilen
37KB
Originalformat Blame Verlauf 

  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. 	{ 0x054C, 0x0268, PS3, true}, 

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

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

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

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

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

  49. };

  50. 

  51. 

  52. 

  53. //-----------------------------------------------------------------------------

  54. void JoystickController::init()

  55. {

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

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

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

  59. 	driver_ready_for_device(this);

  60. 	USBHIDParser::driver_ready_for_hid_collection(this);

  61. 	BluetoothController::driver_ready_for_bluetooth(this);

  62. }

  63. 

  64. //-----------------------------------------------------------------------------

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

  66. {

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

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

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

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

  71. 			return pid_vid_mapping[i].joyType;

  72. 		}

  73. 	}  

  74. 	return UNKNOWN; 	// Not in our list

  75. }

  76. 

  77. //*****************************************************************************

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

  79. //*****************************************************************************

  80. 

  81. uint16_t JoystickController::idVendor() 

  82. {

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

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

  85. 	return 0;

  86. }

  87. 

  88. uint16_t JoystickController::idProduct() 

  89. {

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

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

  92. 	return 0;

  93. }

  94. 

  95. const uint8_t *JoystickController::manufacturer()

  96. {

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

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

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

  100. 	return nullptr;

  101. }

  102. 

  103. const uint8_t *JoystickController::product()

  104. {

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

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

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

  108. 	return nullptr;

  109. }

  110. 

  111. const uint8_t *JoystickController::serialNumber()

  112. {

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

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

  115. 	return nullptr;

  116. }

  117. 

  118. 

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

  120. {

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

  122. 	// joystick types. 

  123. 	rumble_lValue_ = lValue; 

  124. 	rumble_rValue_ = rValue;

  125. 	rumble_timeout_ = timeout;

  126. 

  127. 	switch (joystickType_) {

  128. 		default:

  129. 			break;

  130. 		case PS3:

  131. 			return transmitPS3UserFeedbackMsg();

  132. 		case PS3_MOTION:

  133. 			return transmitPS3MotionUserFeedbackMsg();

  134. 		case PS4:

  135. 			return transmitPS4UserFeedbackMsg();

  136. 		case XBOXONE:

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

  138. 			txbuf_[0] = 0x9;

  139. 			txbuf_[1] = 0x0;

  140. 			txbuf_[2] = 0x0;

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

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

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

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

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

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

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

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

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

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

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

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

  153. 			}

  154. 			return true;	// 

  155. 		case XBOX360:

  156. 			txbuf_[0] = 0x00;

  157. 			txbuf_[1] = 0x01;

  158. 			txbuf_[2] = 0x0F;

  159. 			txbuf_[3] = 0xC0;

  160. 			txbuf_[4] = 0x00;

  161. 			txbuf_[5] = lValue;

  162. 			txbuf_[6] = rValue;

  163. 			txbuf_[7] = 0x00;

  164. 			txbuf_[8] = 0x00;

  165. 			txbuf_[9] = 0x00;

  166. 			txbuf_[10] = 0x00;

  167. 			txbuf_[11] = 0x00;

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

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

  170. 			}

  171. 			return true;

  172. 	} 

  173. 	return false;

  174. }

  175. 

  176. 

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

  178. {

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

  180. 	// joystick types. 

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

  182. 		leds_[0] = lr;

  183. 		leds_[1] = lg;

  184. 		leds_[2] = lb;

  185. 

  186. 		switch (joystickType_) {

  187. 			case PS3:

  188. 				return transmitPS3UserFeedbackMsg();

  189. 			case PS3_MOTION:

  190. 				return transmitPS3MotionUserFeedbackMsg();

  191. 			case PS4:

  192. 				return transmitPS4UserFeedbackMsg();

  193. 			case XBOX360:

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

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

  196. 				// 6-9() - On 

  197. 			    // ...

  198. 				txbuf_[1] = 0x00;

  199. 				txbuf_[2] = 0x08;

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

  201. 				txbuf_[4] = 0x00;

  202. 				txbuf_[5] = 0x00;

  203. 				txbuf_[6] = 0x00;

  204. 				txbuf_[7] = 0x00;

  205. 				txbuf_[8] = 0x00;

  206. 				txbuf_[9] = 0x00;

  207. 				txbuf_[10] = 0x00;

  208. 				txbuf_[11] = 0x00;

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

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

  211. 				}

  212. 				return true;

  213. 			case XBOXONE:

  214. 			default:

  215. 				return false;

  216. 		} 

  217. 	}

  218. 	return false;

  219. }

  220. 

  221. bool JoystickController::transmitPS4UserFeedbackMsg() {

  222. 	if (driver_)  {

  223. 		uint8_t packet[32];

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

  225. 

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

  227. 	    packet[1]= 0xFF;

  228. 

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

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

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

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

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

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

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

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

  237. 	} else if (btdriver_) {

  238. 		uint8_t packet[79];

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

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

  241. 	    packet[0] = 0x52; 

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

  243. 		packet[2] = 0x80;

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

  245. 		packet[4] = 0xFF;

  246. 

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

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

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

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

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

  252. 

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

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

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

  256. 

  257.      	return true;

  258. 	}

  259. 	return false;

  260. }

  261. 

  262. static const uint8_t PS3_USER_FEEDBACK_INIT[] = {

  263.         0x00, 0x00, 0x00, 0x00, 0x00,

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

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

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

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

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

  269.         0x00, 0x00, 0x00, 0x00, 0x00,

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

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

  272.         0x00, 0x00, 0x00 };

  273. 

  274. bool JoystickController::transmitPS3UserFeedbackMsg() {

  275. 	if (driver_) {

  276. 	    memcpy(txbuf_, PS3_USER_FEEDBACK_INIT, 48);

  277. 

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

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

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

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

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

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

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

  285. 	} else if (btdriver_) {

  286. 		txbuf_[0] = 0x52;

  287. 		txbuf_[1] = 0x1;

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

  289. 

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

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

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

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

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

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

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

  297.      	return true;

  298. 	}

  299. 	return false;

  300. }

  301. 

  302. #define MOVE_REPORT_BUFFER_SIZE 7

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

  304. 

  305. bool JoystickController::transmitPS3MotionUserFeedbackMsg() {

  306. 	if (driver_) {

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

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

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

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

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

  312. 

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

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

  315. 

  316. 	} else if (btdriver_) {

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

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

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

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

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

  322.         txbuf_[7] = rumble_lValue_;

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

  324.      	return true;

  325. 	}

  326. 	return false;

  327. }

  328. 

  329. //*****************************************************************************

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

  331. //*****************************************************************************

  332. 

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

  334. {

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

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

  337. 	// only claim from one physical device

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

  339. 

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

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

  342. 

  343. 	mydevice = dev;

  344. 	collections_claimed++;

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

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

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

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

  349. 

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

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

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

  353. 	switch (joystickType_) {

  354. 		case PS3:

  355. 		case PS3_MOTION: // not sure yet

  356. 			additional_axis_usage_page_ = 0x1;

  357. 			additional_axis_usage_start_ = 0x100;

  358. 			additional_axis_usage_count_ = 39;

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

  360. 			break;

  361. 		case PS4:

  362. 			additional_axis_usage_page_ = 0xFF00;

  363. 			additional_axis_usage_start_ = 0x21;

  364. 			additional_axis_usage_count_ = 54;

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

  366. 			break;

  367. 		default: 

  368. 			additional_axis_usage_page_ = 0x09;

  369. 			additional_axis_usage_start_ = 0x21;

  370. 			additional_axis_usage_count_ = 5;

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

  372. 	}

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

  374. 	return CLAIM_REPORT;

  375. }

  376. 

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

  378. {

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

  380. 		mydevice = NULL;

  381. 		driver_ = nullptr;

  382. 		axis_mask_ = 0;	

  383. 		axis_changed_mask_ = 0;

  384. 	}

  385. }

  386. 

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

  388. {

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

  390. }

  391. 

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

  393. {

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

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

  396. 	uint32_t usage_page = usage >> 16;

  397. 	usage &= 0xFFFF;

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

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

  400. 		if (value == 0) {

  401. 			if (buttons & bit) {

  402. 				buttons &= ~bit;

  403. 				anychange = true;

  404. 			}

  405. 		} else {

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

  407. 				buttons |= bit;

  408. 				anychange = true;

  409. 			}

  410. 		}

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

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

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

  414. 		uint32_t i = usage - 0x30;

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

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

  417. 			axis[i] = value;

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

  419. 			if (axis_changed_mask_ & axis_change_notify_mask_)

  420. 				anychange = true;

  421. 		}

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

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

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

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

  426. 			// We are in the user range. 

  427. 			uint16_t usage_index = usage - additional_axis_usage_start_ + STANDARD_AXIS_COUNT;

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

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

  430. 					axis[usage_index] = value;

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

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

  433. 					if (axis_changed_mask_ & axis_change_notify_mask_)

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

  435. 				}

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

  437. 			}

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

  439. 		}

  440. 

  441. 	} else {

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

  443. 

  444. 	}

  445. 	// TODO: hat switch?

  446. }

  447. 

  448. void JoystickController::hid_input_end()

  449. {

  450. 	if (anychange) {

  451. 		joystickEvent = true;

  452. 	}

  453. }

  454. 

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

  456. {

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

  458. 	return true;

  459. }

  460. 

  461. void JoystickController::joystickDataClear() {

  462. 	joystickEvent = false;

  463. 	anychange = false;

  464. 	axis_changed_mask_ = 0;

  465. 	axis_mask_ = 0;

  466. }

  467. 

  468. //*****************************************************************************

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

  470. // Example: XBox One controller. 

  471. //*****************************************************************************

  472. 

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

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

  475. 

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

  477. {

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

  479. 

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

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

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

  483. 

  484. 	// Try claiming at the interface level.

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

  486. 	print_hexbytes(descriptors, len);

  487. 

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

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

  490. 	if (jtype == UNKNOWN)

  491. 		return false; 

  492. 

  493. 	// XBOX One

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

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

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

  497. 

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

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

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

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

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

  503. 

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

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

  506. 

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

  508. 

  509. 	// Some common stuff for both XBoxs

  510. 	uint32_t count_end_points = descriptors[4];

  511. 	if (count_end_points < 2) return false;

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

  513. 	rx_ep_ = 0;

  514. 	uint32_t txep = 0;

  515. 	uint8_t rx_interval = 0;

  516. 	uint8_t tx_interval = 0;

  517. 	rx_size_ = 0;

  518. 	tx_size_ = 0;

  519. 	uint32_t descriptor_index = 9; 

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

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

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

  523. 	}	

  524. 	while (count_end_points-- && ((rx_ep_ == 0) || txep == 0)) {

  525. 		if (descriptors[descriptor_index] != 7) return false; // length 7

  526. 		if (descriptors[descriptor_index+1] != 5) return false; // ep desc

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

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

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

  530. 			// have a bulk EP size 

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

  532. 				rx_ep_ = descriptors[descriptor_index+2];

  533. 				rx_size_ = descriptors[descriptor_index+4];

  534. 				rx_interval = descriptors[descriptor_index+6];

  535. 			} else {

  536. 				txep = descriptors[descriptor_index+2]; 

  537. 				tx_size_ = descriptors[descriptor_index+4];

  538. 				tx_interval = descriptors[descriptor_index+6];

  539. 			}

  540. 		}

  541. 		descriptor_index += 7;  // setup to look at next one...

  542. 	}

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

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

  545. 	print("(", rx_size_);

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

  547. 	print("(", tx_size_);

  548. 	println(")");

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

  550. 	if (!rxpipe_) return false;

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

  552. 	if (!txpipe_) {

  553. 		//free_Pipe(rxpipe_);

  554. 		return false;

  555. 	}

  556. 	rxpipe_->callback_function = rx_callback;

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

  558. 

  559. 	txpipe_->callback_function = tx_callback;

  560. 

  561. 	if (jtype == XBOXONE) {

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

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

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

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

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

  567. 	}

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

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

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

  571. 	return true;

  572. }

  573. 

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

  575. {

  576. }

  577. 

  578. 

  579. /************************************************************/

  580. //  Interrupt-based Data Movement

  581. /************************************************************/

  582. 

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

  584. {

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

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

  587. }

  588. 

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

  590. {

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

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

  593. }

  594. 

  595. 

  596. 

  597. /************************************************************/

  598. //  Interrupt-based Data Movement

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

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

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

  602. /************************************************************/

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

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

  605. typedef struct {

  606. 	uint8_t type;

  607. 	uint8_t const_0;

  608. 	uint16_t id;

  609. 	// From online references button order: 

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

  611. 	//     dpad up, down left, right

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

  613. 	// Axis: 

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

  615. 	//     

  616. 	uint16_t buttons; 

  617. 	int16_t	axis[6];

  618. } xbox1data20_t;

  619. 

  620. typedef struct {

  621. 	uint8_t state;

  622. 	uint8_t id_or_type;

  623. 	uint16_t controller_status;

  624. 	uint16_t unknown;

  625. 	// From online references button order: 

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

  627. 	//     dpad up, down left, right

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

  629. 	// Axis: 

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

  631. 	//

  632. 	uint16_t buttons; 

  633. 	uint8_t lt;

  634. 	uint8_t rt;

  635. 	int16_t	axis[4];

  636. } xbox360data_t;

  637. 

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

  639. 

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

  641. {

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

  643. 	print("): ");

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

  645. 

  646. 	if (joystickType_ == XBOXONE) {

  647. 		// Process XBOX One data

  648. 		axis_mask_ = 0x3f;	

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

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

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

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

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

  654. 				buttons = xb1d->buttons;

  655. 				anychange = true;

  656. 				joystickEvent = true;

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

  658. 			}

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

  660. 				// The first two values were unsigned. 

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

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

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

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

  665. 					anychange = true;

  666. 				}

  667. 			}

  668. 			joystickEvent = true;

  669. 		}

  670. 

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

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

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

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

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

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

  677. 				if (connected_) {

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

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

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

  681. 

  682. 				} else {

  683. 					println("XBox360w - disconnected");

  684. 				}

  685. 			}

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

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

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

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

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

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

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

  693. 	        

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

  695. 	        	buttons = xb360d->buttons;

  696. 	        	anychange = true;

  697. 	        }

  698. 			axis_mask_ = 0x3f;	

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

  700. 

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

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

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

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

  705. 					anychange = true;

  706. 				}

  707. 			}

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

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

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

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

  712. 				anychange = true;

  713. 			}

  714. 

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

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

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

  718. 				anychange = true;

  719. 			}

  720. 

  721. 			if (anychange) joystickEvent = true;

  722. 		}

  723. 	}

  724. 

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

  726. }

  727. 

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

  729. {

  730. }

  731. 

  732. void JoystickController::disconnect()

  733. {

  734. 	axis_mask_ = 0;	

  735. 	axis_changed_mask_ = 0;

  736. 	// TODO: free resources

  737. }

  738. 

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

  740. {

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

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

  743. 		btdriver_ = driver;

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

  745. 		if (remoteName) mapNameToJoystickType(remoteName);

  746. 		return true;

  747. 	}

  748. 

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

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

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

  752. 			btdriver_ = driver;

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

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

  755. 			return true;

  756. 		}

  757. 	}

  758. 	return false;

  759. }

  760. 

  761. 

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

  763. {

  764. 	// Example data from PS4 controller

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

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

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

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

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

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

  771. 		// print_hexbytes(data, length);

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

  773. 		DBGPrintf("  Joystick Data: ");

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

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

  776. 		if (joystickType_ == PS3) {

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

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

  779. 			if (cur_buttons != buttons) {

  780. 				buttons = cur_buttons;

  781. 				joystickEvent = true;	// something changed.

  782. 			}

  783. 

  784. 			uint64_t mask = 0x1;

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

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

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

  788. 					axis_changed_mask_ |= mask;

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

  790. 				}

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

  792. 			}

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

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

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

  796. 			}

  797. 			

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

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

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

  801. 			}

  802. 			

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

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

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

  806. 			}

  807. 			

  808. 			// Then rest of data

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

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

  811. 				axis_mask_ |= mask;

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

  813. 					axis_changed_mask_ |= mask;

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

  815. 				} 

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

  817. 			}

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

  819. 			// Quick and dirty PS3_Motion data.

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

  821. 			if (cur_buttons != buttons) {

  822. 				buttons = cur_buttons;

  823. 				joystickEvent = true;	// something changed.

  824. 			}

  825. 

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

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

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

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

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

  831. 			// 6 - Time stamp

  832. 			// 7 - Battery

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

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

  835. 			// 32 - Temp High

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

  837. 			uint64_t mask = 0x1;

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

  839. 			// Then rest of data

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

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

  842. 				axis_mask_ |= mask;

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

  844. 					axis_changed_mask_ |= mask;

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

  846. 				} 

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

  848. 			}

  849. 

  850. 		} else {

  851. 			uint64_t mask = 0x1;

  852. 			axis_mask_ = 0;

  853. 

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

  855. 				axis_mask_ |= mask;

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

  857. 					axis_changed_mask_ |= mask;

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

  859. 				} 

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

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

  862. 			}

  863. 

  864. 		}

  865. 

  866. 		if (axis_changed_mask_ & axis_change_notify_mask_)

  867. 			joystickEvent = true;

  868. 		connected_ = true;

  869. 		return true;

  870. 

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

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

  873. 		uint64_t mask = 0x1;

  874. 		axis_mask_ = 0;

  875. 		axis_changed_mask_ = 0;

  876. 		

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

  878. 		//data[0] = 0x01

  879. 		uint8_t tmp_data[length-2];

  880. 		

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

  882. 			tmp_data[i] = 0;

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

  884. 		}

  885. 		

  886. 		/*

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

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

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

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

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

  892. 		 * [10-11] Timestamp

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

  894. 		 * [13-14] acceleration x

  895. 		 * [15-16] acceleration y

  896. 		 * [17-18] acceleration z

  897. 		 * [19-20] gyro x

  898. 		 * [21-22] gyro y

  899. 		 * [23-24] gyro z

  900. 		 * [25-29] unknown

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

  902. 		 * rest is trackpad?  to do implement?

  903. 		 */

  904. 		//PS Bit

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

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

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

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

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

  910. 		

  911. 	

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

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

  914. 		if (cur_buttons != buttons) {

  915. 			buttons = cur_buttons;

  916. 			joystickEvent = true;	// something changed.

  917. 		}

  918. 		

  919. 		mask = 0x1;

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

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

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

  923. 				axis_changed_mask_ |= mask;

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

  925. 			}

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

  927. 		}

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

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

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

  931. 		}

  932. 		

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

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

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

  936. 		}

  937. 		

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

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

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

  941. 		}

  942. 		

  943. 		//limit for masking

  944. 		mask = 0x1;

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

  946. 			axis_mask_ |= mask;

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

  948. 				axis_changed_mask_ |= mask;

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

  950. 			}

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

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

  953. 		}

  954. 		DBGPrintf("\n");

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

  956. 		joystickEvent = true;

  957. 		connected_ = true;

  958. 	}

  959. 	return false;

  960. }

  961. 

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

  963. {

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

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

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

  967. 		joystickType_ = PS4;

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

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

  970. 		joystickType_ = PS3;

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

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

  973. 		joystickType_ = PS3;

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

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

  976. 		joystickType_ = PS3_MOTION;

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

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

  979. 		joystickType_ = XBOXONE;

  980. 	} else {

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

  982. 	}

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

  984. 	return true;

  985. }

  986. 

  987. 

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

  989. {

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

  991. 	if (mapNameToJoystickType(remoteName)) {

  992. 		switch (joystickType_) {

  993. 			case PS4: special_process_required = SP_NEED_CONNECT; break;

  994. 			case PS3: special_process_required = SP_PS3_IDS; break;

  995. 			case PS3_MOTION: special_process_required = SP_PS3_IDS; break;

  996. 			default: 

  997. 				break;

  998. 		}

  999. 	}

  1000. 	return true;

  1001. }

  1002. 

  1003. void JoystickController::connectionComplete() 

  1004. {

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

  1006. 	switch (joystickType_) {

  1007. 	case PS4:

  1008. 		{

  1009. 			uint8_t packet[2];

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

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

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

  1013. 			delay(1);

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

  1015. 		}

  1016. 		break;

  1017. 	case PS3:

  1018. 		{

  1019. 			uint8_t packet[6];

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

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

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

  1023. 			packet[3] = 0x03;

  1024. 			packet[4] = 0x00;

  1025. 			packet[5] = 0x00;

  1026. 

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

  1028. 			delay(1);

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

  1030. 		}

  1031. 		break;

  1032. 	case PS3_MOTION:	

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

  1034. 	default:

  1035. 		break;	

  1036. 	}

  1037. }

  1038. 

  1039. void JoystickController::release_bluetooth() 

  1040. {

  1041. 	btdevice = nullptr;	// remember this way 

  1042. 	btdriver_ = nullptr;

  1043. 	connected_ = false;

  1044. 	special_process_required = false;

  1045. 

  1046. }

  1047. 

  1048. 

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

  1050. 	if (!driver_) return false;

  1051.  	if (joystickType_ == PS3) {

  1052.  	    /* Set the internal Bluetooth address */

  1053. 	    txbuf_[0] = 0x01;

  1054. 	    txbuf_[1] = 0x00;

  1055. 

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

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

  1058. 

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

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

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

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

  1063. 	    txbuf_[0] = 0x05;

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

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

  1066. 

  1067.         txbuf_[7] = 0x10;

  1068.         txbuf_[8] = 0x01;

  1069.         txbuf_[9] = 0x02;

  1070.         txbuf_[10] = 0x12;

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

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

  1073. 	}

  1074. 	return false;

  1075. }