source: flair-src/trunk/tools/Controller/DualShock3/src/DualShock3.cpp@ 468

Last change on this file since 468 was 467, checked in by Sanahuja Guillaume, 3 years ago

change default usb method for ds3, now input event, no need to be root
File size: 25.1 KB
RevLine 
[11]1// %flair:license{
[16]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[11]4// %flair:license}
5// created: 2015/03/30
6// filename: DualShock3.h
7//
8// author: Gildas Bayard
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
[16]13// purpose: Base class for host side remote controls that talks to target
14// side through ethernet connection
[11]15//
16//
17/*********************************************************************/
18#include "DualShock3.h"
19#include <Controller.h>
[214]20#include <Matrix.h>
[11]21#include <Tab.h>
22#include <TabWidget.h>
23#include <CheckBox.h>
24#include <Label.h>
25#include <DataPlot1D.h>
26#include <SpinBox.h>
27#include <GroupBox.h>
28#include <FrameworkManager.h>
29#include <sstream>
30
31#include <stdlib.h>
32#include <unistd.h>
33#include <errno.h>
34#include <fcntl.h>
35#include <sstream>
36#include <linux/input.h>
37#include <linux/hidraw.h>
38#include <bluetooth/hci.h>
39#include <bluetooth/hci_lib.h>
[290]40#include <sys/file.h>
[467]41#include <linux/input.h>
[11]42
43#define USB_DIR_IN 0x80
44#define USB_DIR_OUT 0
45#define USB_GET_REPORT 0x01
46#define USB_SET_REPORT 0x09
47#define VENDOR_SONY 0x054c
48#define PRODUCT_SIXAXIS_DS3 0x0268
49
50#define L2CAP_PSM_HIDP_CTRL 0x11
51#define L2CAP_PSM_HIDP_INTR 0x13
52
[16]53#define HIDP_TRANS_GET_REPORT 0x40
54#define HIDP_TRANS_SET_REPORT 0x50
55#define HIDP_DATA_RTYPE_OUTPUT 0x02
56#define HIDP_DATA_RTYPE_FEATURE 0x03
[11]57
58#define SIXAXIS 1
59#define DUALSHOCK3 2
60
61#define X_AXIS_RANGE 127
62#define Y_AXIS_RANGE 127
63
64using namespace flair::core;
65using namespace flair::gui;
66using namespace std;
67
68typedef struct motion_dev {
[16]69 int index;
70 bdaddr_t addr;
71 char type;
72 int csk;
73 int isk;
74 struct motion_dev *next;
[11]75} motion_dev_t;
76
[16]77bdaddr_t bdaddr_any = {{0, 0, 0, 0, 0, 0}};
[11]78
[16]79namespace flair {
80namespace sensor {
[11]81
[137]82DualShock3::DualShock3(string name,
[16]83 string receiverAddress, int receiverPort,
84 ConnectionType_t _connectionType, uint32_t period,
85 uint32_t bitsPerAxis, uint8_t priority)
[137]86 : HostEthController(name, receiverAddress, receiverPort, period,
[16]87 bitsPerAxis, priority),
88 connectionType(_connectionType) {
89 controllerName = "DualShock3";
90 last_voltage_time = 0;
[11]91
[16]92 // axis stuff
93 axisNumber = 4;
94 nativeBitsPerAxis = 8;
[318]95 MatrixDescriptor *axisDescriptor = new MatrixDescriptor(axisNumber, 1);
[16]96 for (unsigned int i = 0; i < axisNumber; i++) {
97 axisDescriptor->SetElementName(i, 0, GetAxisDescription(i));
98 }
[214]99 axis = new Matrix((IODevice *)this, axisDescriptor, Int16Type);
[148]100 delete axisDescriptor;
[16]101 AddDataToLog(axis);
[11]102
[16]103 // buttons stuff
104 buttonNumber = 16;
[318]105 MatrixDescriptor *buttonDescriptor =
106 new MatrixDescriptor(buttonNumber, 1);
[16]107 for (unsigned int i = 0; i < buttonNumber; i++) {
108 buttonDescriptor->SetElementName(i, 0, GetButtonDescription(i));
109 }
[214]110 button = new Matrix((IODevice *)this, buttonDescriptor, Int8Type);
[16]111 AddDataToLog(button);
[11]112
[16]113 Tab *settingsTab = new Tab(tabWidget, "Settings");
114 dataSize = 6;
115 datas = new int8_t[dataSize];
[467]116 for(int i=0;i<dataSize;i++) datas[i]=0;
[11]117
[16]118 GroupBox *settingsGroupBox =
119 new GroupBox(settingsTab->NewRow(), controllerName);
120 deadZone =
121 new SpinBox(settingsGroupBox->NewRow(), "dead zone:", -130, 130, 1);
122 batteryChargeLevel =
123 new Label(settingsGroupBox->LastRowLastCol(), "battery charge level");
124 enabled = new CheckBox(settingsGroupBox->LastRowLastCol(), "enabled");
[11]125
[16]126 if (connectionType == Bluetooth) {
127 // init DS3
128 usb_scan();
[11]129
[16]130 int csk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_CTRL);
131 isk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_INTR);
[11]132
[16]133 if (csk >= 0 && isk >= 0)
134 Printf("Waiting for Bluetooth connections.\n");
135 else
136 Thread::Err("Unable to listen on HID PSMs.\n");
[11]137
[16]138 fd_set fds;
139 FD_ZERO(&fds);
[11]140
[16]141 if (csk >= 0)
142 FD_SET(csk, &fds);
143 if (select(csk + 1, &fds, NULL, NULL, NULL) < 0)
144 Thread::Err("select\n");
145 // Incoming connection ?
[11]146
[16]147 if (csk >= 0 && FD_ISSET(csk, &fds)) {
148 // printf("accept\n");
149 dev = accept_device(csk, isk);
150 setup_device(dev);
151 }
[467]152 } else if (connectionType == UsbHidRaw) {
[16]153 int nr, i;
154 unsigned char buf[128];
[11]155
[16]156 for (i = 0; i < 255; i++) {
157 ostringstream dev_name;
158 dev_name << "/dev/hidraw" << i;
159 if ((usb_fd = open(dev_name.str().c_str(), O_RDONLY)) >= 0) {
[290]160 int res=flock(usb_fd, LOCK_EX|LOCK_NB);
161 if(res<0) {
162 Thread::Warn("%s seems to be locked by another application\n", dev_name.str().c_str());
163 close(usb_fd);
164 continue;
165 }
166
[16]167 struct hidraw_devinfo info;
168 res = ioctl(usb_fd, HIDIOCGRAWINFO, &info);
169 if (res < 0) {
[290]170 Thread::Err("ioctl error (HIDIOCGRAWINFO) on %s\n",dev_name.str().c_str());
[16]171 } else {
172 // Printf("%x %x\n", info.vendor, info.product);
173 if (info.vendor == 0x054c && info.product == 0x0268) {
174 Printf("successfully opened %s\n", dev_name.str().c_str());
175 Printf("Press PS button to turn the controller on\n");
176 break;
177 }
[11]178 }
[290]179 flock(usb_fd, LOCK_UN);
[16]180 close(usb_fd);
181 }
182 }
183 if (i == 255) {
[467]184 Thread::Err("open(hidrawX) - failed to open hidraw device\n");
[16]185 return;
186 }
[11]187
[16]188 // block until PS button is pressed
189 if ((nr = read(usb_fd, buf, sizeof(buf))) < 0) {
[467]190 Thread::Err("read(fd) - failed to read from device\n");
[16]191 }
[11]192
[16]193 if (nr < 49 || nr > 50) {
[467]194 Thread::Err("read(fd) - not a sixaxis (nr = %i )\n", nr);
[11]195 }
[467]196 } else if (connectionType == UsbEvent) {
197 int nr, i;
198 unsigned char buf[128];
199
200 for (i = 0; i < 255; i++) {
201 ostringstream dev_name;
202 dev_name << "/dev/input/event" << i;
203 if ((usb_fd = open(dev_name.str().c_str(), O_RDONLY)) >= 0) {
204 int res=flock(usb_fd, LOCK_EX|LOCK_NB);
205 if(res<0) {
206 Thread::Warn("%s seems to be locked by another application\n", dev_name.str().c_str());
207 close(usb_fd);
208 continue;
209 }
210
211 unsigned short id[4];
212 res = ioctl(usb_fd, EVIOCGID, id);
213 if (res < 0) {
214 Thread::Err("ioctl error (EVIOCGID) on %s\n",dev_name.str().c_str());
215 } else {
216 if (id[ID_VENDOR] == 0x054c && id[ID_PRODUCT] == 0x0268) {
217 Printf("successfully opened %s\n", dev_name.str().c_str());
218 Printf("Press PS button to turn the controller on\n");
219 break;
220 }
221 }
222 flock(usb_fd, LOCK_UN);
223 close(usb_fd);
224 }
225 }
226 if (i == 255) {
227 Thread::Err("open(/dev/input/eventX) - failed to open event device\n");
228 Thread::Err("you can try with the old usb_hidraw method\n");
229 return;
230 }
231
232 // block until PS button is pressed
233 struct input_event tmp;
234 if ((nr = read(usb_fd, &tmp, sizeof(struct input_event))) < 0) {
235 Thread::Err("read(fd) - failed to read from device\n");
236 }
[16]237 }
238 ledmask = 0;
[11]239}
240
241DualShock3::~DualShock3() {
[467]242 if (connectionType == UsbHidRaw || connectionType == UsbEvent) {
[290]243 flock(usb_fd, LOCK_UN);
[16]244 close(usb_fd);
245 }
246 if (connectionType == Bluetooth) {
247 if (!popen("/etc/init.d/bluetooth restart", "r"))
248 Thread::Warn("Could not restart bluetooth service\n");
249 }
[11]250}
251
252string DualShock3::GetAxisDescription(unsigned int axis) {
[16]253 string description;
[11]254
[16]255 switch (axis) {
256 case 0:
257 description = "left stick x-axis";
258 break;
259 case 1:
260 description = "left stick y-axis";
261 break;
262 case 2:
263 description = "right stick x-axis";
264 break;
265 case 3:
266 description = "right stick y-axis";
267 break;
268 }
269 return description;
[11]270}
271
272string DualShock3::GetButtonDescription(unsigned int button) {
[16]273 switch (button) {
274 case 0:
275 return "start";
276 break;
277 case 1:
278 return "select";
279 break;
280 case 2:
281 return "square";
282 break;
283 case 3:
284 return "triangle";
285 break;
286 case 4:
287 return "circle";
288 break;
289 case 5:
290 return "cross";
291 break;
292 case 6:
293 return "left 1";
294 break;
295 case 7:
296 return "left 2";
297 break;
298 case 8:
299 return "left 3";
300 break;
301 case 9:
302 return "right 1";
303 break;
304 case 10:
305 return "right 2";
306 break;
307 case 11:
308 return "right 3";
309 break;
310 case 12:
311 return "up";
312 break;
313 case 13:
314 return "down";
315 break;
316 case 14:
317 return "left";
318 break;
319 case 15:
320 return "right";
321 break;
322 }
[11]323}
324
325bool DualShock3::IsDataFrameReady() {
[16]326 unsigned char report[256];
327 unsigned char tmp_report[256];
[11]328
[16]329 if (!enabled->IsChecked()) {
330 meaningfulDataAvailable = false;
331 usleep(100000);
332 return false;
333 }
334 now = GetTime();
335 if (connectionType == Bluetooth) {
336 fd_set fds;
337 FD_ZERO(&fds);
338 int fdmax = 0; /*
339 if ( isk >= 0 ) FD_SET(isk, &fds);
340 if ( isk > fdmax ) fdmax = isk;
[11]341*/
[16]342 FD_SET(dev->isk, &fds);
343 if (dev->isk > fdmax)
344 fdmax = dev->isk;
[11]345
[16]346 if (select(fdmax + 1, &fds, NULL, NULL, NULL) < 0)
347 fatal("select");
[11]348
[16]349 // Incoming input report ?
350 if (FD_ISSET(dev->isk, &fds)) {
351 int nr;
352 int recv_result;
353 bool flushed = false;
354 while (!flushed) {
355 recv_result = recv(dev->isk, tmp_report, sizeof(report), MSG_DONTWAIT);
356 if (recv_result <= 0) {
357 if ((errno != EAGAIN) && (errno != EWOULDBLOCK)) {
358 fprintf(stderr, "%d disconnected\n", dev->index);
359 close(dev->csk);
360 close(dev->isk);
361 free(dev);
362 return false;
363 } else {
364 flushed = true;
365 // fprintf(stderr, "\n");
366 continue;
367 }
368 } else {
369 // fprintf(stderr, ".");
370 nr = recv_result;
371 memcpy(report, tmp_report, nr);
[11]372 }
[16]373 }
374 if (report[0] == 0xa1) {
375 return parse_report_sixaxis_ds3(report + 1, nr - 1);
376 }
[11]377 }
378 return false;
[16]379
[467]380 } else if (connectionType == UsbHidRaw) {
[16]381 int nr = read(usb_fd, report, sizeof(report));
382 return parse_report_sixaxis_ds3(report, nr);
[467]383 } else if (connectionType == UsbEvent) {
384 return parse_input_event();
[16]385 }
386 return false;
[11]387}
388
[467]389uint8_t DualShock3::SetBitValue(int8_t &reg,uint8_t mask,uint8_t value) {
390 reg=(reg&~mask) | value*mask;
391}
392
393bool DualShock3::parse_input_event(void) {
394 struct input_event ev[64];
395 int i, rd;
396
397 rd = read(usb_fd, ev, sizeof(struct input_event) * 64);
398
399 if (rd < (int) sizeof(struct input_event)) {
400 printf("expected %d bytes, got %d\n", (int) sizeof(struct input_event), rd);
401 return false;
402 }
403
404 for (i = 0; i < rd / sizeof(struct input_event); i++) {
405 if (ev[i].type == EV_ABS) {
406 //code 2 and 5 are z/rz (l2 and r2)
407 if(ev[i].code==0) datas[2] = compute_dead_zone(0, ev[i].value);//x
408 if(ev[i].code==1) datas[3] = compute_dead_zone(1, ev[i].value);//y
409 if(ev[i].code==3) datas[4] = compute_dead_zone(2, ev[i].value);//rx
410 if(ev[i].code==4) datas[5] = compute_dead_zone(3, ev[i].value);//ry
411 }
412 if (ev[i].type == EV_KEY) {
413 if(ev[i].code==BTN_START) SetBitValue(datas[0],0x08,ev[i].value);
414 if(ev[i].code==BTN_SELECT) SetBitValue(datas[0],0x01,ev[i].value);
415 if(ev[i].code==BTN_WEST) SetBitValue(datas[1],0x80,ev[i].value);// square
416 if(ev[i].code==BTN_NORTH) SetBitValue(datas[1],0x10,ev[i].value);// triangle
417 if(ev[i].code==BTN_EAST) SetBitValue(datas[1],0x20,ev[i].value);// circle
418 if(ev[i].code==BTN_SOUTH) SetBitValue(datas[1],0x40,ev[i].value);// cross
419 if(ev[i].code==BTN_TL) SetBitValue(datas[1],0x04,ev[i].value);
420 if(ev[i].code==BTN_TL2) SetBitValue(datas[1],0x01,ev[i].value);
421 if(ev[i].code==BTN_THUMBL) SetBitValue(datas[0],0x02,ev[i].value);
422 if(ev[i].code==BTN_TR) SetBitValue(datas[1],0x08,ev[i].value);
423 if(ev[i].code==BTN_TR2) SetBitValue(datas[1],0x02,ev[i].value);
424 if(ev[i].code==BTN_THUMBR) SetBitValue(datas[0],0x04,ev[i].value);
425 if(ev[i].code==BTN_DPAD_UP) SetBitValue(datas[0],0x10,ev[i].value);
426 if(ev[i].code==BTN_DPAD_DOWN) SetBitValue(datas[0],0x40,ev[i].value);
427 if(ev[i].code==BTN_DPAD_LEFT) SetBitValue(datas[0],0x80,ev[i].value);
428 if(ev[i].code==BTN_DPAD_RIGHT) SetBitValue(datas[0],0x20,ev[i].value);
429 }
430
431 if (GetTime() > (last_voltage_time + 5 * (Time)1000000000)) {
432 // toute les 5 secondes
433 // report battery charge level
434 batteryChargeLevel->SetText("battery: usb connected");
435 last_voltage_time = GetTime();
436 }
437 }
438 return true;
439}
440
[11]441bool DualShock3::parse_report_sixaxis_ds3(unsigned char *r, int len) {
[16]442 if (r[0] == 0x01 && r[1] == 0 && len >= 49) {
443 datas[0] = r[2];
444 datas[1] = r[3];
445 datas[2] = compute_dead_zone(0, r[6]);
446 datas[3] = compute_dead_zone(1, r[7]);
447 datas[4] = compute_dead_zone(2, r[8]);
448 datas[5] = compute_dead_zone(3, r[9]);
[11]449
[16]450 if (GetTime() > (last_voltage_time + 5 * (Time)1000000000)) {
451 // toute les 5 secondes
452 // report battery charge level
453 if (connectionType == Bluetooth) {
454 batteryChargeLevel->SetText("battery: %i/5", r[30]);
455 }
[467]456 if (connectionType == UsbHidRaw) {
[16]457 batteryChargeLevel->SetText("battery: usb connected");
458 }
459 last_voltage_time = GetTime();
460 }
[11]461
[16]462 return true;
463 }
464 return false;
[11]465}
466
467void DualShock3::GetAxisData() {
468
[16]469 axis->GetMutex();
470 // axis->SetValueNoMutex(0, 0,datas[2]/(float)X_AXIS_RANGE); //left stick
471 // x-axis
472 // axis->SetValueNoMutex(1, 0,datas[3]/(float)Y_AXIS_RANGE); //left stick
473 // y-axis
474 // axis->SetValueNoMutex(2, 0,datas[4]/(float)X_AXIS_RANGE); //right stick
475 // x-axis
476 // axis->SetValueNoMutex(3, 0,datas[5]/(float)Y_AXIS_RANGE); //right stick
477 // y-axis
478 axis->SetValueNoMutex(0, 0, datas[2]); // left stick x-axis
479 axis->SetValueNoMutex(1, 0, datas[3]); // left stick y-axis
480 axis->SetValueNoMutex(2, 0, datas[4]); // right stick x-axis
481 axis->SetValueNoMutex(3, 0, datas[5]); // right stick y-axis
482 axis->ReleaseMutex();
483 axis->SetDataTime(now);
[11]484}
485
486void DualShock3::GetButtonData() {
[16]487 button->GetMutex();
488 button->SetValueNoMutex(0, 0, (datas[0] & 0x08) == 0 ? 0 : 1); // start
489 button->SetValueNoMutex(1, 0, (datas[0] & 0x01) == 0 ? 0 : 1); // select
490 button->SetValueNoMutex(2, 0, (datas[1] & 0x80) == 0 ? 0 : 1); // square
491 button->SetValueNoMutex(3, 0, (datas[1] & 0x10) == 0 ? 0 : 1); // triangle
492 button->SetValueNoMutex(4, 0, (datas[1] & 0x20) == 0 ? 0 : 1); // circle
493 button->SetValueNoMutex(5, 0, (datas[1] & 0x40) == 0 ? 0 : 1); // cross
494 button->SetValueNoMutex(6, 0, (datas[1] & 0x04) == 0 ? 0 : 1); // left 1
495 button->SetValueNoMutex(7, 0, (datas[1] & 0x01) == 0 ? 0 : 1); // left 2
496 button->SetValueNoMutex(8, 0, (datas[0] & 0x02) == 0 ? 0 : 1); // left 3
497 button->SetValueNoMutex(9, 0, (datas[1] & 0x08) == 0 ? 0 : 1); // right 1
498 button->SetValueNoMutex(10, 0, (datas[1] & 0x02) == 0 ? 0 : 1); // right 2
499 button->SetValueNoMutex(11, 0, (datas[0] & 0x04) == 0 ? 0 : 1); // right 3
500 button->SetValueNoMutex(12, 0, (datas[0] & 0x10) == 0 ? 0 : 1); // up
501 button->SetValueNoMutex(13, 0, (datas[0] & 0x40) == 0 ? 0 : 1); // down
502 button->SetValueNoMutex(14, 0, (datas[0] & 0x80) == 0 ? 0 : 1); // left
503 button->SetValueNoMutex(15, 0, (datas[0] & 0x20) == 0 ? 0 : 1); // right
504 button->ReleaseMutex();
505 button->SetDataTime(now);
[11]506}
507
508void DualShock3::ProcessMessage(core::Message *controllerAction) {
[16]509 ControllerAction action;
510 memcpy(&action, controllerAction->buffer, sizeof(ControllerAction));
511 if (action == ControllerAction::SetLedOn) {
512 Thread::Info("LedOn action request\n");
513 } else if (action == ControllerAction::SetLedOff) {
514 Thread::Info("LedOff action request\n");
515 } else if (action == ControllerAction::Rumble) {
516 Thread::Info("Rumble action request\n");
517 } else if (action == ControllerAction::FlashLed) {
518 Thread::Info("FlashLed action request\n");
519 }
520 // (char *msg, int msgSize)
521 /* for (unsigned int i=0; i<4; i++) {
522 if(msg[4+2*i]!=0 || msg[5+2*i]!=0) set_led(i+1,msg[4+2*i],msg[5+2*i]);
[11]523
[16]524 }
525 if(msg[0]!=0 || msg[2]!=0) rumble(msg[0],msg[1],msg[2],msg[3]);
526 */
[11]527}
528
529// ----------------------------------------------------------------------
530// Replacement for libbluetooth
531
532int DualShock3::mystr2ba(const char *s, bdaddr_t *ba) {
[16]533 if (strlen(s) != 17)
534 return 1;
535 for (int i = 0; i < 6; ++i) {
536 int d = strtol(s + 15 - 3 * i, NULL, 16);
537 if (d < 0 || d > 255)
538 return 1;
539 ba->b[i] = d;
540 }
541 return 0;
[11]542}
543
544char *DualShock3::myba2str(const bdaddr_t *ba) {
[16]545 static char buf[2][18]; // Static buffer valid for two invocations.
546 static int index = 0;
547 index = (index + 1) % 2;
548 sprintf(buf[index], "%02x:%02x:%02x:%02x:%02x:%02x", ba->b[5], ba->b[4],
549 ba->b[3], ba->b[2], ba->b[1], ba->b[0]);
550 return buf[index];
[11]551}
552
553void DualShock3::fatal(const char *msg) {
[16]554 if (errno)
555 perror(msg);
556 else
557 fprintf(stderr, "%s\n", msg);
558 exit(1);
[11]559}
560
561/**********************************************************************/
562// Bluetooth HID devices
563// Incoming connections.
564
565int DualShock3::l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm) {
[16]566 int sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
567 if (sk < 0)
568 fatal("socket");
[11]569
[16]570 struct sockaddr_l2 addr;
571 addr.l2_family = AF_BLUETOOTH;
572 addr.l2_bdaddr = *BDADDR_ANY;
573 addr.l2_psm = htobs(psm);
574 addr.l2_cid = 0;
[11]575
[16]576 if (bind(sk, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
577 close(sk);
[307]578 char errorMsg[256];
579 Thread::Err("bth bind, %s\n", strerror_r(errno, errorMsg, sizeof(errorMsg)));
580 return -1;
[16]581 }
[11]582
[16]583 if (listen(sk, 5) < 0)
584 fatal("listen");
585 return sk;
[11]586}
587
588struct motion_dev *DualShock3::accept_device(int csk, int isk) {
[16]589 Printf("Incoming connection...\n");
590 struct motion_dev *dev = (motion_dev *)malloc(sizeof(struct motion_dev));
591 if (!dev)
592 fatal("malloc");
[11]593
[16]594 dev->csk = accept(csk, NULL, NULL);
595 if (dev->csk < 0)
596 fatal("accept(CTRL)");
597 dev->isk = accept(isk, NULL, NULL);
598 if (dev->isk < 0)
599 fatal("accept(INTR)");
[11]600
[16]601 struct sockaddr_l2 addr;
602 socklen_t addrlen = sizeof(addr);
603 if (getpeername(dev->isk, (struct sockaddr *)&addr, &addrlen) < 0)
604 fatal("getpeername");
605 dev->addr = addr.l2_bdaddr;
[11]606
[16]607 // Distinguish SIXAXIS / DS3
608 unsigned char resp[64];
609 char get03f2[] = {HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_FEATURE | 8,
610 (char)0xf2, sizeof(resp), sizeof(resp) >> 8};
611 send(dev->csk, get03f2, sizeof(get03f2), 0); // 0301 is interesting too.
612 int nr = recv(dev->csk, resp, sizeof(resp), 0);
[11]613
[16]614 dev->type = (resp[13] == 0x40) ? SIXAXIS : DUALSHOCK3; // My guess.
[11]615
[16]616 return dev;
[11]617}
618
619/**********************************************************************/
620// Device setup
621
622#define IR0 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x00, 0x41
623#define IR1 0x40, 0x00
624
625#define BIT1 2
626
627void DualShock3::hidp_trans(int csk, char *buf, int len) {
[16]628 if (send(csk, buf, len, 0) != len)
629 fatal("send(CTRL)");
630 char ack;
631 int nr = recv(csk, &ack, sizeof(ack), 0);
632 if (nr != 1 || ack != 0)
633 fatal("ack");
[11]634}
635
636void DualShock3::setup_device(struct motion_dev *dev) {
637
[16]638 switch (dev->type) {
639 case SIXAXIS:
640 case DUALSHOCK3:
641 // Enable reporting
642 char set03f4[] = {HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE,
643 (char)0xf4, 0x42, 0x03, 0x00, 0x00};
644 hidp_trans(dev->csk, set03f4, sizeof(set03f4));
[11]645
[16]646 break;
647 }
[11]648}
649
650/**********************************************************************/
651// Reports
652
[16]653// faire un reglage station sol pour dead zone
654int8_t DualShock3::compute_dead_zone(int axis, unsigned char value) {
655 float tmp;
[11]656
[16]657 if (value > 128 + deadZone->Value()) {
658 tmp =
659 (value - deadZone->Value() - 128.) * 128. / (128. - deadZone->Value());
660 } else if (value < 128 - deadZone->Value()) {
661 // return value+DEAD_ZONE-128;
662 tmp =
663 (value + deadZone->Value() - 128.) * 127. / (128. - deadZone->Value());
664 } else {
665 return 0;
666 }
[11]667
[16]668 if (tmp > 127)
669 return 127;
670 if (tmp < -127)
671 return -127;
672 if (tmp > ((int8_t)tmp + .5) && tmp > 0)
673 return (int8_t)(tmp + 1);
674 if (tmp < ((int8_t)tmp - .5) && tmp < 0)
675 return (int8_t)(tmp - 1);
[11]676
[16]677 return (int8_t)tmp;
[11]678}
679
680/**********************************************************************/
681// USB functions
682
[16]683void DualShock3::usb_pair_device(struct usb_device *dev, int itfnum) {
[11]684
[16]685 usb_dev_handle *devh = usb_open(dev);
686 if (!devh)
687 fatal("usb_open");
688 usb_detach_kernel_driver_np(devh, itfnum);
689 int res = usb_claim_interface(devh, itfnum);
690 if (res < 0)
691 fatal("usb_claim_interface");
[11]692
[16]693 bdaddr_t current_ba; // Current pairing address.
[11]694
[16]695 switch (dev->descriptor.idProduct) {
696 case PRODUCT_SIXAXIS_DS3: {
697 // remote_printf("USB: SIXAXIS/DS3\n");
698 char msg[8];
699 res =
700 usb_control_msg(devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
701 USB_GET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
702 /*
703 unsigned char msg[8];
704 res = usb_control_msg
705 (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
706 USB_GET_REPORT, 0x03f5, itfnum, (void*)msg, sizeof(msg),
707 5000);*/
708 if (res < 0)
709 fatal("usb_control_msg(read master)");
710 for (int i = 0; i < 6; ++i)
711 current_ba.b[i] = (uint8_t)msg[7 - i];
712 break;
713 }
714 }
[11]715
[16]716 // New pairing address
717 int dev_id;
718 dev_id = hci_get_route(NULL);
719 struct hci_dev_info di;
720 hci_devinfo(dev_id, &di);
[11]721
[16]722 // Perform pairing.
723 if (!bacmp(&current_ba, &di.bdaddr)) {
724 printf(" Already paired to %s\n", myba2str(&di.bdaddr));
725 } else {
726 printf(" Changing master from %s to %s\n", myba2str(&current_ba),
727 myba2str(&di.bdaddr));
728 switch (dev->descriptor.idProduct) {
729 case PRODUCT_SIXAXIS_DS3: {
730 char msg[8] = {0x01, 0x00, (char)di.bdaddr.b[5], (char)di.bdaddr.b[4],
731 (char)di.bdaddr.b[3], (char)di.bdaddr.b[2],
732 (char)di.bdaddr.b[1], (char)di.bdaddr.b[0]};
733 res = usb_control_msg(
734 devh, USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
735 USB_SET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
736 if (res < 0)
737 fatal("usb_control_msg(write master)");
738 break;
[11]739 }
740 }
[16]741 }
[11]742
[16]743 if (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3)
744 printf(" Now unplug the USB cable and press the PS button.\n");
745 else
746 printf(" Now press the PS button.\n");
[11]747}
748
[16]749void DualShock3::usb_scan() {
750 usb_init();
751 if (usb_find_busses() < 0)
752 fatal("usb_find_busses");
753 if (usb_find_devices() < 0)
754 fatal("usb_find_devices");
755 struct usb_bus *busses = usb_get_busses();
756 if (!busses)
757 fatal("usb_get_busses");
[11]758
[16]759 struct usb_bus *bus;
760 for (bus = busses; bus; bus = bus->next) {
761 struct usb_device *dev;
762 for (dev = bus->devices; dev; dev = dev->next) {
763 struct usb_config_descriptor *cfg;
764 for (cfg = dev->config;
765 cfg < dev->config + dev->descriptor.bNumConfigurations; ++cfg) {
766 int itfnum;
767 for (itfnum = 0; itfnum < cfg->bNumInterfaces; ++itfnum) {
768 struct usb_interface *itf = &cfg->interface[itfnum];
769 struct usb_interface_descriptor *alt;
770 for (alt = itf->altsetting;
771 alt < itf->altsetting + itf->num_altsetting; ++alt) {
772 if (dev->descriptor.idVendor == VENDOR_SONY &&
773 (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3) &&
774 alt->bInterfaceClass == 3)
775 usb_pair_device(dev, itfnum);
776 }
[11]777 }
[16]778 }
[11]779 }
[16]780 }
[11]781}
782
[16]783void DualShock3::rumble(uint8_t left_force, uint8_t left_timeout,
784 uint8_t right_force, uint8_t right_timeout) {
785 // printf("rumble\n");
[11]786
[16]787 unsigned char datas[] = {
788 0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */, 0x01, 0x00, 0x00,
789 0x00, 0x00, 0x00, // rumble values
790 0x00, 0x00, 0x00, 0x00, (unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4
[11]791
[16]792 0xff, 0x27, led4_on, led4_off, 0x32, 0xff, 0x27, led3_on, led3_off, 0x32,
793 0xff, 0x27, led2_on, led2_off, 0x32, 0xff, 0x27, led1_on, led1_off, 0x32,
794 0x00, 0x00, 0x00, 0x00, 0x00,
795 };
[11]796
[16]797 datas[5] = left_timeout; // left timeout
798 datas[6] = left_force; // left force
799 datas[3] = right_timeout; // right timeout
800 datas[4] = right_force; // right force
[11]801
[16]802 if (connectionType == Bluetooth) {
803 hidp_trans(dev->csk, (char *)datas, sizeof(datas));
804 }
[11]805}
806
[16]807void DualShock3::set_led(uint8_t led, uint8_t on_timeout, uint8_t off_timeout) {
808 uint8_t mask;
[11]809
[16]810 switch (led) {
811 case 1:
812 led1_on = on_timeout;
813 led1_off = off_timeout;
814 mask = 2;
815 break;
816 case 2:
817 led2_on = on_timeout;
818 led2_off = off_timeout;
819 mask = 4;
820 break;
821 case 3:
822 led3_on = on_timeout;
823 led3_off = off_timeout;
824 mask = 8;
825 break;
826 case 4:
827 led4_on = on_timeout;
828 led4_off = off_timeout;
829 mask = 16;
830 break;
831 }
[11]832
[16]833 if (on_timeout != 0) {
834 ledmask |= mask;
835 } else {
836 ledmask &= ~mask;
837 }
838 /*
839 printf("led %x\n",ledmask);
840 printf("1:%i %i\n",led1_on,led1_off);
841 printf("2:%i %i\n",led2_on,led2_off);
842 printf("3:%i %i\n",led3_on,led3_off);
843 printf("4:%i %i\n",led4_on,led4_off);*/
[11]844
[16]845 unsigned char datas[] = {
846 0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */, 0x01, 0x00, 0x00,
847 0x00, 0x00, 0x00, // rumble values
848 0x00, 0x00, 0x00, 0x00, (unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4
[11]849
[16]850 0xff, 0x27, led4_on, led4_off, 0x32, 0xff, 0x27, led3_on, led3_off, 0x32,
851 0xff, 0x27, led2_on, led2_off, 0x32, 0xff, 0x27, led1_on, led1_off, 0x32,
852 0x00, 0x00, 0x00, 0x00, 0x00,
853 };
[11]854
[16]855 if (connectionType == Bluetooth) {
856 hidp_trans(dev->csk, (char *)datas, sizeof(datas));
857 }
[11]858}
859
860} // end namespace sensor
861} // end namespace flair
Note: See TracBrowser for help on using the repository browser.