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

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

add timeout for ds3 input event
File size: 25.4 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
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//
13// purpose: Base class for host side remote controls that talks to target
14// side through ethernet connection
15//
16//
17/*********************************************************************/
18#include "DualShock3.h"
19#include <Controller.h>
20#include <Matrix.h>
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>
40#include <sys/file.h>
41#include <linux/input.h>
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
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
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 {
69 int index;
70 bdaddr_t addr;
71 char type;
72 int csk;
73 int isk;
74 struct motion_dev *next;
75} motion_dev_t;
76
77bdaddr_t bdaddr_any = {{0, 0, 0, 0, 0, 0}};
78
79namespace flair {
80namespace sensor {
81
82DualShock3::DualShock3(string name,
83 string receiverAddress, int receiverPort,
84 ConnectionType_t _connectionType, uint32_t period,
85 uint32_t bitsPerAxis, uint8_t priority)
86 : HostEthController(name, receiverAddress, receiverPort, period,
87 bitsPerAxis, priority),
88 connectionType(_connectionType) {
89 controllerName = "DualShock3";
90 last_voltage_time = 0;
91
92 // axis stuff
93 axisNumber = 4;
94 nativeBitsPerAxis = 8;
95 MatrixDescriptor *axisDescriptor = new MatrixDescriptor(axisNumber, 1);
96 for (unsigned int i = 0; i < axisNumber; i++) {
97 axisDescriptor->SetElementName(i, 0, GetAxisDescription(i));
98 }
99 axis = new Matrix((IODevice *)this, axisDescriptor, Int16Type);
100 delete axisDescriptor;
101 AddDataToLog(axis);
102
103 // buttons stuff
104 buttonNumber = 16;
105 MatrixDescriptor *buttonDescriptor =
106 new MatrixDescriptor(buttonNumber, 1);
107 for (unsigned int i = 0; i < buttonNumber; i++) {
108 buttonDescriptor->SetElementName(i, 0, GetButtonDescription(i));
109 }
110 button = new Matrix((IODevice *)this, buttonDescriptor, Int8Type);
111 AddDataToLog(button);
112
113 Tab *settingsTab = new Tab(tabWidget, "Settings");
114 dataSize = 6;
115 datas = new int8_t[dataSize];
116 for(int i=0;i<dataSize;i++) datas[i]=0;
117
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");
125
126 if (connectionType == Bluetooth) {
127 // init DS3
128 usb_scan();
129
130 int csk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_CTRL);
131 isk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_INTR);
132
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");
137
138 fd_set fds;
139 FD_ZERO(&fds);
140
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 ?
146
147 if (csk >= 0 && FD_ISSET(csk, &fds)) {
148 // printf("accept\n");
149 dev = accept_device(csk, isk);
150 setup_device(dev);
151 }
152 } else if (connectionType == UsbHidRaw) {
153 int nr, i;
154 unsigned char buf[128];
155
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) {
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
167 struct hidraw_devinfo info;
168 res = ioctl(usb_fd, HIDIOCGRAWINFO, &info);
169 if (res < 0) {
170 Thread::Err("ioctl error (HIDIOCGRAWINFO) on %s\n",dev_name.str().c_str());
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 }
178 }
179 flock(usb_fd, LOCK_UN);
180 close(usb_fd);
181 }
182 }
183 if (i == 255) {
184 Thread::Err("open(hidrawX) - failed to open hidraw device\n");
185 return;
186 }
187
188 // block until PS button is pressed
189 if ((nr = read(usb_fd, buf, sizeof(buf))) < 0) {
190 Thread::Err("read(fd) - failed to read from device\n");
191 }
192
193 if (nr < 49 || nr > 50) {
194 Thread::Err("read(fd) - not a sixaxis (nr = %i )\n", nr);
195 }
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 }
237 }
238 ledmask = 0;
239}
240
241DualShock3::~DualShock3() {
242 if (connectionType == UsbHidRaw || connectionType == UsbEvent) {
243 flock(usb_fd, LOCK_UN);
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 }
250}
251
252string DualShock3::GetAxisDescription(unsigned int axis) {
253 string description;
254
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;
270}
271
272string DualShock3::GetButtonDescription(unsigned int button) {
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 }
323}
324
325bool DualShock3::IsDataFrameReady() {
326 unsigned char report[256];
327 unsigned char tmp_report[256];
328
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;
341*/
342 FD_SET(dev->isk, &fds);
343 if (dev->isk > fdmax)
344 fdmax = dev->isk;
345
346 if (select(fdmax + 1, &fds, NULL, NULL, NULL) < 0)
347 fatal("select");
348
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);
372 }
373 }
374 if (report[0] == 0xa1) {
375 return parse_report_sixaxis_ds3(report + 1, nr - 1);
376 }
377 }
378 return false;
379
380 } else if (connectionType == UsbHidRaw) {
381 int nr = read(usb_fd, report, sizeof(report));
382 return parse_report_sixaxis_ds3(report, nr);
383 } else if (connectionType == UsbEvent) {
384 return parse_input_event(100);
385 }
386 return false;
387}
388
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(int timeout_ms) {
394 struct input_event ev[64];
395 int i, rd;
396 fd_set set;
397 struct timeval timeout;
398
399 FD_ZERO(&set); // clear the set
400 FD_SET(usb_fd, &set); // add our file descriptor to the set
401
402 timeout.tv_sec = 0;
403 timeout.tv_usec = timeout_ms * 1000;
404
405 int rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
406
407 if (rv <=0) {
408 return false;
409 }
410
411 rd = read(usb_fd, ev, sizeof(struct input_event) * 64);
412
413 if (rd < (int) sizeof(struct input_event)) {
414 printf("expected %d bytes, got %d\n", (int) sizeof(struct input_event), rd);
415 return false;
416 }
417
418 for (i = 0; i < rd / sizeof(struct input_event); i++) {
419 if (ev[i].type == EV_ABS) {
420 //code 2 and 5 are z/rz (l2 and r2)
421 if(ev[i].code==0) datas[2] = compute_dead_zone(0, ev[i].value);//x
422 if(ev[i].code==1) datas[3] = compute_dead_zone(1, ev[i].value);//y
423 if(ev[i].code==3) datas[4] = compute_dead_zone(2, ev[i].value);//rx
424 if(ev[i].code==4) datas[5] = compute_dead_zone(3, ev[i].value);//ry
425 }
426 if (ev[i].type == EV_KEY) {
427 if(ev[i].code==BTN_START) SetBitValue(datas[0],0x08,ev[i].value);
428 if(ev[i].code==BTN_SELECT) SetBitValue(datas[0],0x01,ev[i].value);
429 if(ev[i].code==BTN_WEST) SetBitValue(datas[1],0x80,ev[i].value);// square
430 if(ev[i].code==BTN_NORTH) SetBitValue(datas[1],0x10,ev[i].value);// triangle
431 if(ev[i].code==BTN_EAST) SetBitValue(datas[1],0x20,ev[i].value);// circle
432 if(ev[i].code==BTN_SOUTH) SetBitValue(datas[1],0x40,ev[i].value);// cross
433 if(ev[i].code==BTN_TL) SetBitValue(datas[1],0x04,ev[i].value);
434 if(ev[i].code==BTN_TL2) SetBitValue(datas[1],0x01,ev[i].value);
435 if(ev[i].code==BTN_THUMBL) SetBitValue(datas[0],0x02,ev[i].value);
436 if(ev[i].code==BTN_TR) SetBitValue(datas[1],0x08,ev[i].value);
437 if(ev[i].code==BTN_TR2) SetBitValue(datas[1],0x02,ev[i].value);
438 if(ev[i].code==BTN_THUMBR) SetBitValue(datas[0],0x04,ev[i].value);
439 if(ev[i].code==BTN_DPAD_UP) SetBitValue(datas[0],0x10,ev[i].value);
440 if(ev[i].code==BTN_DPAD_DOWN) SetBitValue(datas[0],0x40,ev[i].value);
441 if(ev[i].code==BTN_DPAD_LEFT) SetBitValue(datas[0],0x80,ev[i].value);
442 if(ev[i].code==BTN_DPAD_RIGHT) SetBitValue(datas[0],0x20,ev[i].value);
443 }
444
445 if (GetTime() > (last_voltage_time + 5 * (Time)1000000000)) {
446 // toute les 5 secondes
447 // report battery charge level
448 batteryChargeLevel->SetText("battery: usb connected");
449 last_voltage_time = GetTime();
450 }
451 }
452 return true;
453}
454
455bool DualShock3::parse_report_sixaxis_ds3(unsigned char *r, int len) {
456 if (r[0] == 0x01 && r[1] == 0 && len >= 49) {
457 datas[0] = r[2];
458 datas[1] = r[3];
459 datas[2] = compute_dead_zone(0, r[6]);
460 datas[3] = compute_dead_zone(1, r[7]);
461 datas[4] = compute_dead_zone(2, r[8]);
462 datas[5] = compute_dead_zone(3, r[9]);
463
464 if (GetTime() > (last_voltage_time + 5 * (Time)1000000000)) {
465 // toute les 5 secondes
466 // report battery charge level
467 if (connectionType == Bluetooth) {
468 batteryChargeLevel->SetText("battery: %i/5", r[30]);
469 }
470 if (connectionType == UsbHidRaw) {
471 batteryChargeLevel->SetText("battery: usb connected");
472 }
473 last_voltage_time = GetTime();
474 }
475
476 return true;
477 }
478 return false;
479}
480
481void DualShock3::GetAxisData() {
482
483 axis->GetMutex();
484 // axis->SetValueNoMutex(0, 0,datas[2]/(float)X_AXIS_RANGE); //left stick
485 // x-axis
486 // axis->SetValueNoMutex(1, 0,datas[3]/(float)Y_AXIS_RANGE); //left stick
487 // y-axis
488 // axis->SetValueNoMutex(2, 0,datas[4]/(float)X_AXIS_RANGE); //right stick
489 // x-axis
490 // axis->SetValueNoMutex(3, 0,datas[5]/(float)Y_AXIS_RANGE); //right stick
491 // y-axis
492 axis->SetValueNoMutex(0, 0, datas[2]); // left stick x-axis
493 axis->SetValueNoMutex(1, 0, datas[3]); // left stick y-axis
494 axis->SetValueNoMutex(2, 0, datas[4]); // right stick x-axis
495 axis->SetValueNoMutex(3, 0, datas[5]); // right stick y-axis
496 axis->ReleaseMutex();
497 axis->SetDataTime(now);
498}
499
500void DualShock3::GetButtonData() {
501 button->GetMutex();
502 button->SetValueNoMutex(0, 0, (datas[0] & 0x08) == 0 ? 0 : 1); // start
503 button->SetValueNoMutex(1, 0, (datas[0] & 0x01) == 0 ? 0 : 1); // select
504 button->SetValueNoMutex(2, 0, (datas[1] & 0x80) == 0 ? 0 : 1); // square
505 button->SetValueNoMutex(3, 0, (datas[1] & 0x10) == 0 ? 0 : 1); // triangle
506 button->SetValueNoMutex(4, 0, (datas[1] & 0x20) == 0 ? 0 : 1); // circle
507 button->SetValueNoMutex(5, 0, (datas[1] & 0x40) == 0 ? 0 : 1); // cross
508 button->SetValueNoMutex(6, 0, (datas[1] & 0x04) == 0 ? 0 : 1); // left 1
509 button->SetValueNoMutex(7, 0, (datas[1] & 0x01) == 0 ? 0 : 1); // left 2
510 button->SetValueNoMutex(8, 0, (datas[0] & 0x02) == 0 ? 0 : 1); // left 3
511 button->SetValueNoMutex(9, 0, (datas[1] & 0x08) == 0 ? 0 : 1); // right 1
512 button->SetValueNoMutex(10, 0, (datas[1] & 0x02) == 0 ? 0 : 1); // right 2
513 button->SetValueNoMutex(11, 0, (datas[0] & 0x04) == 0 ? 0 : 1); // right 3
514 button->SetValueNoMutex(12, 0, (datas[0] & 0x10) == 0 ? 0 : 1); // up
515 button->SetValueNoMutex(13, 0, (datas[0] & 0x40) == 0 ? 0 : 1); // down
516 button->SetValueNoMutex(14, 0, (datas[0] & 0x80) == 0 ? 0 : 1); // left
517 button->SetValueNoMutex(15, 0, (datas[0] & 0x20) == 0 ? 0 : 1); // right
518 button->ReleaseMutex();
519 button->SetDataTime(now);
520}
521
522void DualShock3::ProcessMessage(core::Message *controllerAction) {
523 ControllerAction action;
524 memcpy(&action, controllerAction->buffer, sizeof(ControllerAction));
525 if (action == ControllerAction::SetLedOn) {
526 Thread::Info("LedOn action request\n");
527 } else if (action == ControllerAction::SetLedOff) {
528 Thread::Info("LedOff action request\n");
529 } else if (action == ControllerAction::Rumble) {
530 Thread::Info("Rumble action request\n");
531 } else if (action == ControllerAction::FlashLed) {
532 Thread::Info("FlashLed action request\n");
533 }
534 // (char *msg, int msgSize)
535 /* for (unsigned int i=0; i<4; i++) {
536 if(msg[4+2*i]!=0 || msg[5+2*i]!=0) set_led(i+1,msg[4+2*i],msg[5+2*i]);
537
538 }
539 if(msg[0]!=0 || msg[2]!=0) rumble(msg[0],msg[1],msg[2],msg[3]);
540 */
541}
542
543// ----------------------------------------------------------------------
544// Replacement for libbluetooth
545
546int DualShock3::mystr2ba(const char *s, bdaddr_t *ba) {
547 if (strlen(s) != 17)
548 return 1;
549 for (int i = 0; i < 6; ++i) {
550 int d = strtol(s + 15 - 3 * i, NULL, 16);
551 if (d < 0 || d > 255)
552 return 1;
553 ba->b[i] = d;
554 }
555 return 0;
556}
557
558char *DualShock3::myba2str(const bdaddr_t *ba) {
559 static char buf[2][18]; // Static buffer valid for two invocations.
560 static int index = 0;
561 index = (index + 1) % 2;
562 sprintf(buf[index], "%02x:%02x:%02x:%02x:%02x:%02x", ba->b[5], ba->b[4],
563 ba->b[3], ba->b[2], ba->b[1], ba->b[0]);
564 return buf[index];
565}
566
567void DualShock3::fatal(const char *msg) {
568 if (errno)
569 perror(msg);
570 else
571 fprintf(stderr, "%s\n", msg);
572 exit(1);
573}
574
575/**********************************************************************/
576// Bluetooth HID devices
577// Incoming connections.
578
579int DualShock3::l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm) {
580 int sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
581 if (sk < 0)
582 fatal("socket");
583
584 struct sockaddr_l2 addr;
585 addr.l2_family = AF_BLUETOOTH;
586 addr.l2_bdaddr = *BDADDR_ANY;
587 addr.l2_psm = htobs(psm);
588 addr.l2_cid = 0;
589
590 if (bind(sk, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
591 close(sk);
592 char errorMsg[256];
593 Thread::Err("bth bind, %s\n", strerror_r(errno, errorMsg, sizeof(errorMsg)));
594 return -1;
595 }
596
597 if (listen(sk, 5) < 0)
598 fatal("listen");
599 return sk;
600}
601
602struct motion_dev *DualShock3::accept_device(int csk, int isk) {
603 Printf("Incoming connection...\n");
604 struct motion_dev *dev = (motion_dev *)malloc(sizeof(struct motion_dev));
605 if (!dev)
606 fatal("malloc");
607
608 dev->csk = accept(csk, NULL, NULL);
609 if (dev->csk < 0)
610 fatal("accept(CTRL)");
611 dev->isk = accept(isk, NULL, NULL);
612 if (dev->isk < 0)
613 fatal("accept(INTR)");
614
615 struct sockaddr_l2 addr;
616 socklen_t addrlen = sizeof(addr);
617 if (getpeername(dev->isk, (struct sockaddr *)&addr, &addrlen) < 0)
618 fatal("getpeername");
619 dev->addr = addr.l2_bdaddr;
620
621 // Distinguish SIXAXIS / DS3
622 unsigned char resp[64];
623 char get03f2[] = {HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_FEATURE | 8,
624 (char)0xf2, sizeof(resp), sizeof(resp) >> 8};
625 send(dev->csk, get03f2, sizeof(get03f2), 0); // 0301 is interesting too.
626 int nr = recv(dev->csk, resp, sizeof(resp), 0);
627
628 dev->type = (resp[13] == 0x40) ? SIXAXIS : DUALSHOCK3; // My guess.
629
630 return dev;
631}
632
633/**********************************************************************/
634// Device setup
635
636#define IR0 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x00, 0x41
637#define IR1 0x40, 0x00
638
639#define BIT1 2
640
641void DualShock3::hidp_trans(int csk, char *buf, int len) {
642 if (send(csk, buf, len, 0) != len)
643 fatal("send(CTRL)");
644 char ack;
645 int nr = recv(csk, &ack, sizeof(ack), 0);
646 if (nr != 1 || ack != 0)
647 fatal("ack");
648}
649
650void DualShock3::setup_device(struct motion_dev *dev) {
651
652 switch (dev->type) {
653 case SIXAXIS:
654 case DUALSHOCK3:
655 // Enable reporting
656 char set03f4[] = {HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE,
657 (char)0xf4, 0x42, 0x03, 0x00, 0x00};
658 hidp_trans(dev->csk, set03f4, sizeof(set03f4));
659
660 break;
661 }
662}
663
664/**********************************************************************/
665// Reports
666
667// faire un reglage station sol pour dead zone
668int8_t DualShock3::compute_dead_zone(int axis, unsigned char value) {
669 float tmp;
670
671 if (value > 128 + deadZone->Value()) {
672 tmp =
673 (value - deadZone->Value() - 128.) * 128. / (128. - deadZone->Value());
674 } else if (value < 128 - deadZone->Value()) {
675 // return value+DEAD_ZONE-128;
676 tmp =
677 (value + deadZone->Value() - 128.) * 127. / (128. - deadZone->Value());
678 } else {
679 return 0;
680 }
681
682 if (tmp > 127)
683 return 127;
684 if (tmp < -127)
685 return -127;
686 if (tmp > ((int8_t)tmp + .5) && tmp > 0)
687 return (int8_t)(tmp + 1);
688 if (tmp < ((int8_t)tmp - .5) && tmp < 0)
689 return (int8_t)(tmp - 1);
690
691 return (int8_t)tmp;
692}
693
694/**********************************************************************/
695// USB functions
696
697void DualShock3::usb_pair_device(struct usb_device *dev, int itfnum) {
698
699 usb_dev_handle *devh = usb_open(dev);
700 if (!devh)
701 fatal("usb_open");
702 usb_detach_kernel_driver_np(devh, itfnum);
703 int res = usb_claim_interface(devh, itfnum);
704 if (res < 0)
705 fatal("usb_claim_interface");
706
707 bdaddr_t current_ba; // Current pairing address.
708
709 switch (dev->descriptor.idProduct) {
710 case PRODUCT_SIXAXIS_DS3: {
711 // remote_printf("USB: SIXAXIS/DS3\n");
712 char msg[8];
713 res =
714 usb_control_msg(devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
715 USB_GET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
716 /*
717 unsigned char msg[8];
718 res = usb_control_msg
719 (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
720 USB_GET_REPORT, 0x03f5, itfnum, (void*)msg, sizeof(msg),
721 5000);*/
722 if (res < 0)
723 fatal("usb_control_msg(read master)");
724 for (int i = 0; i < 6; ++i)
725 current_ba.b[i] = (uint8_t)msg[7 - i];
726 break;
727 }
728 }
729
730 // New pairing address
731 int dev_id;
732 dev_id = hci_get_route(NULL);
733 struct hci_dev_info di;
734 hci_devinfo(dev_id, &di);
735
736 // Perform pairing.
737 if (!bacmp(&current_ba, &di.bdaddr)) {
738 printf(" Already paired to %s\n", myba2str(&di.bdaddr));
739 } else {
740 printf(" Changing master from %s to %s\n", myba2str(&current_ba),
741 myba2str(&di.bdaddr));
742 switch (dev->descriptor.idProduct) {
743 case PRODUCT_SIXAXIS_DS3: {
744 char msg[8] = {0x01, 0x00, (char)di.bdaddr.b[5], (char)di.bdaddr.b[4],
745 (char)di.bdaddr.b[3], (char)di.bdaddr.b[2],
746 (char)di.bdaddr.b[1], (char)di.bdaddr.b[0]};
747 res = usb_control_msg(
748 devh, USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
749 USB_SET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
750 if (res < 0)
751 fatal("usb_control_msg(write master)");
752 break;
753 }
754 }
755 }
756
757 if (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3)
758 printf(" Now unplug the USB cable and press the PS button.\n");
759 else
760 printf(" Now press the PS button.\n");
761}
762
763void DualShock3::usb_scan() {
764 usb_init();
765 if (usb_find_busses() < 0)
766 fatal("usb_find_busses");
767 if (usb_find_devices() < 0)
768 fatal("usb_find_devices");
769 struct usb_bus *busses = usb_get_busses();
770 if (!busses)
771 fatal("usb_get_busses");
772
773 struct usb_bus *bus;
774 for (bus = busses; bus; bus = bus->next) {
775 struct usb_device *dev;
776 for (dev = bus->devices; dev; dev = dev->next) {
777 struct usb_config_descriptor *cfg;
778 for (cfg = dev->config;
779 cfg < dev->config + dev->descriptor.bNumConfigurations; ++cfg) {
780 int itfnum;
781 for (itfnum = 0; itfnum < cfg->bNumInterfaces; ++itfnum) {
782 struct usb_interface *itf = &cfg->interface[itfnum];
783 struct usb_interface_descriptor *alt;
784 for (alt = itf->altsetting;
785 alt < itf->altsetting + itf->num_altsetting; ++alt) {
786 if (dev->descriptor.idVendor == VENDOR_SONY &&
787 (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3) &&
788 alt->bInterfaceClass == 3)
789 usb_pair_device(dev, itfnum);
790 }
791 }
792 }
793 }
794 }
795}
796
797void DualShock3::rumble(uint8_t left_force, uint8_t left_timeout,
798 uint8_t right_force, uint8_t right_timeout) {
799 // printf("rumble\n");
800
801 unsigned char datas[] = {
802 0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */, 0x01, 0x00, 0x00,
803 0x00, 0x00, 0x00, // rumble values
804 0x00, 0x00, 0x00, 0x00, (unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4
805
806 0xff, 0x27, led4_on, led4_off, 0x32, 0xff, 0x27, led3_on, led3_off, 0x32,
807 0xff, 0x27, led2_on, led2_off, 0x32, 0xff, 0x27, led1_on, led1_off, 0x32,
808 0x00, 0x00, 0x00, 0x00, 0x00,
809 };
810
811 datas[5] = left_timeout; // left timeout
812 datas[6] = left_force; // left force
813 datas[3] = right_timeout; // right timeout
814 datas[4] = right_force; // right force
815
816 if (connectionType == Bluetooth) {
817 hidp_trans(dev->csk, (char *)datas, sizeof(datas));
818 }
819}
820
821void DualShock3::set_led(uint8_t led, uint8_t on_timeout, uint8_t off_timeout) {
822 uint8_t mask;
823
824 switch (led) {
825 case 1:
826 led1_on = on_timeout;
827 led1_off = off_timeout;
828 mask = 2;
829 break;
830 case 2:
831 led2_on = on_timeout;
832 led2_off = off_timeout;
833 mask = 4;
834 break;
835 case 3:
836 led3_on = on_timeout;
837 led3_off = off_timeout;
838 mask = 8;
839 break;
840 case 4:
841 led4_on = on_timeout;
842 led4_off = off_timeout;
843 mask = 16;
844 break;
845 }
846
847 if (on_timeout != 0) {
848 ledmask |= mask;
849 } else {
850 ledmask &= ~mask;
851 }
852 /*
853 printf("led %x\n",ledmask);
854 printf("1:%i %i\n",led1_on,led1_off);
855 printf("2:%i %i\n",led2_on,led2_off);
856 printf("3:%i %i\n",led3_on,led3_off);
857 printf("4:%i %i\n",led4_on,led4_off);*/
858
859 unsigned char datas[] = {
860 0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */, 0x01, 0x00, 0x00,
861 0x00, 0x00, 0x00, // rumble values
862 0x00, 0x00, 0x00, 0x00, (unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4
863
864 0xff, 0x27, led4_on, led4_off, 0x32, 0xff, 0x27, led3_on, led3_off, 0x32,
865 0xff, 0x27, led2_on, led2_off, 0x32, 0xff, 0x27, led1_on, led1_off, 0x32,
866 0x00, 0x00, 0x00, 0x00, 0x00,
867 };
868
869 if (connectionType == Bluetooth) {
870 hidp_trans(dev->csk, (char *)datas, sizeof(datas));
871 }
872}
873
874} // end namespace sensor
875} // end namespace flair
Note: See TracBrowser for help on using the repository browser.