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

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

support for multiple ds3 in usb

File size: 21.6 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 <Socket.h>
30#include <sstream>
31
32#include <stdlib.h>
33#include <unistd.h>
34#include <errno.h>
35#include <fcntl.h>
36#include <sstream>
37#include <linux/input.h>
38#include <linux/hidraw.h>
39#include <bluetooth/hci.h>
40#include <bluetooth/hci_lib.h>
41#include <sys/file.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 cvmatrix_descriptor *axisDescriptor = new cvmatrix_descriptor(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 cvmatrix_descriptor *buttonDescriptor =
106 new cvmatrix_descriptor(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
117 GroupBox *settingsGroupBox =
118 new GroupBox(settingsTab->NewRow(), controllerName);
119 deadZone =
120 new SpinBox(settingsGroupBox->NewRow(), "dead zone:", -130, 130, 1);
121 batteryChargeLevel =
122 new Label(settingsGroupBox->LastRowLastCol(), "battery charge level");
123 enabled = new CheckBox(settingsGroupBox->LastRowLastCol(), "enabled");
124
125 if (connectionType == Bluetooth) {
126 // init DS3
127 usb_scan();
128
129 int csk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_CTRL);
130 isk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_INTR);
131
132 if (csk >= 0 && isk >= 0)
133 Printf("Waiting for Bluetooth connections.\n");
134 else
135 Thread::Err("Unable to listen on HID PSMs.\n");
136
137 fd_set fds;
138 FD_ZERO(&fds);
139
140 if (csk >= 0)
141 FD_SET(csk, &fds);
142 if (select(csk + 1, &fds, NULL, NULL, NULL) < 0)
143 Thread::Err("select\n");
144 // Incoming connection ?
145
146 if (csk >= 0 && FD_ISSET(csk, &fds)) {
147 // printf("accept\n");
148 dev = accept_device(csk, isk);
149 setup_device(dev);
150 }
151 } else if (connectionType == Usb) {
152 int nr, i;
153 unsigned char buf[128];
154
155 for (i = 0; i < 255; i++) {
156 ostringstream dev_name;
157 dev_name << "/dev/hidraw" << i;
158 if ((usb_fd = open(dev_name.str().c_str(), O_RDONLY)) >= 0) {
159 int res=flock(usb_fd, LOCK_EX|LOCK_NB);
160 if(res<0) {
161 Thread::Warn("%s seems to be locked by another application\n", dev_name.str().c_str());
162 close(usb_fd);
163 continue;
164 }
165
166 struct hidraw_devinfo info;
167 res = ioctl(usb_fd, HIDIOCGRAWINFO, &info);
168 if (res < 0) {
169 Thread::Err("ioctl error (HIDIOCGRAWINFO) on %s\n",dev_name.str().c_str());
170 } else {
171 // Printf("%x %x\n", info.vendor, info.product);
172 if (info.vendor == 0x054c && info.product == 0x0268) {
173 Printf("successfully opened %s\n", dev_name.str().c_str());
174 Printf("Press PS button to turn the controller on\n");
175 break;
176 }
177 }
178 flock(usb_fd, LOCK_UN);
179 close(usb_fd);
180 }
181 }
182 if (i == 255) {
183 Thread::Err("sixad-raw::open(hidrawX) - failed to open hidraw device\n");
184 return;
185 }
186
187 // block until PS button is pressed
188 if ((nr = read(usb_fd, buf, sizeof(buf))) < 0) {
189 Thread::Err("sixad-raw::read(fd) - failed to read from device\n");
190 }
191
192 if (nr < 49 || nr > 50) {
193 Thread::Err("sixad-raw::read(fd) - not a sixaxis (nr = %i )\n", nr);
194 }
195 }
196 ledmask = 0;
197}
198
199DualShock3::~DualShock3() {
200 if (connectionType == Usb) {
201 flock(usb_fd, LOCK_UN);
202 close(usb_fd);
203 }
204 if (connectionType == Bluetooth) {
205 if (!popen("/etc/init.d/bluetooth restart", "r"))
206 Thread::Warn("Could not restart bluetooth service\n");
207 }
208}
209
210string DualShock3::GetAxisDescription(unsigned int axis) {
211 string description;
212
213 switch (axis) {
214 case 0:
215 description = "left stick x-axis";
216 break;
217 case 1:
218 description = "left stick y-axis";
219 break;
220 case 2:
221 description = "right stick x-axis";
222 break;
223 case 3:
224 description = "right stick y-axis";
225 break;
226 }
227 return description;
228}
229
230string DualShock3::GetButtonDescription(unsigned int button) {
231 switch (button) {
232 case 0:
233 return "start";
234 break;
235 case 1:
236 return "select";
237 break;
238 case 2:
239 return "square";
240 break;
241 case 3:
242 return "triangle";
243 break;
244 case 4:
245 return "circle";
246 break;
247 case 5:
248 return "cross";
249 break;
250 case 6:
251 return "left 1";
252 break;
253 case 7:
254 return "left 2";
255 break;
256 case 8:
257 return "left 3";
258 break;
259 case 9:
260 return "right 1";
261 break;
262 case 10:
263 return "right 2";
264 break;
265 case 11:
266 return "right 3";
267 break;
268 case 12:
269 return "up";
270 break;
271 case 13:
272 return "down";
273 break;
274 case 14:
275 return "left";
276 break;
277 case 15:
278 return "right";
279 break;
280 }
281}
282
283bool DualShock3::IsDataFrameReady() {
284 unsigned char report[256];
285 unsigned char tmp_report[256];
286
287 if (!enabled->IsChecked()) {
288 meaningfulDataAvailable = false;
289 usleep(100000);
290 return false;
291 }
292 now = GetTime();
293 if (connectionType == Bluetooth) {
294 fd_set fds;
295 FD_ZERO(&fds);
296 int fdmax = 0; /*
297 if ( isk >= 0 ) FD_SET(isk, &fds);
298 if ( isk > fdmax ) fdmax = isk;
299*/
300 FD_SET(dev->isk, &fds);
301 if (dev->isk > fdmax)
302 fdmax = dev->isk;
303
304 if (select(fdmax + 1, &fds, NULL, NULL, NULL) < 0)
305 fatal("select");
306
307 // Incoming input report ?
308 if (FD_ISSET(dev->isk, &fds)) {
309 int nr;
310 int recv_result;
311 bool flushed = false;
312 while (!flushed) {
313 recv_result = recv(dev->isk, tmp_report, sizeof(report), MSG_DONTWAIT);
314 if (recv_result <= 0) {
315 if ((errno != EAGAIN) && (errno != EWOULDBLOCK)) {
316 fprintf(stderr, "%d disconnected\n", dev->index);
317 close(dev->csk);
318 close(dev->isk);
319 free(dev);
320 return false;
321 } else {
322 flushed = true;
323 // fprintf(stderr, "\n");
324 continue;
325 }
326 } else {
327 // fprintf(stderr, ".");
328 nr = recv_result;
329 memcpy(report, tmp_report, nr);
330 }
331 }
332 if (report[0] == 0xa1) {
333 return parse_report_sixaxis_ds3(report + 1, nr - 1);
334 }
335 }
336 return false;
337
338 } else if (connectionType == Usb) {
339 int nr = read(usb_fd, report, sizeof(report));
340 return parse_report_sixaxis_ds3(report, nr);
341 }
342 return false;
343}
344
345bool DualShock3::parse_report_sixaxis_ds3(unsigned char *r, int len) {
346 if (r[0] == 0x01 && r[1] == 0 && len >= 49) {
347 datas[0] = r[2];
348 datas[1] = r[3];
349 datas[2] = compute_dead_zone(0, r[6]);
350 datas[3] = compute_dead_zone(1, r[7]);
351 datas[4] = compute_dead_zone(2, r[8]);
352 datas[5] = compute_dead_zone(3, r[9]);
353
354 if (GetTime() > (last_voltage_time + 5 * (Time)1000000000)) {
355 // toute les 5 secondes
356 // report battery charge level
357 if (connectionType == Bluetooth) {
358 batteryChargeLevel->SetText("battery: %i/5", r[30]);
359 }
360 if (connectionType == Usb) {
361 batteryChargeLevel->SetText("battery: usb connected");
362 }
363 last_voltage_time = GetTime();
364 }
365
366 return true;
367 }
368 return false;
369}
370
371void DualShock3::GetAxisData() {
372
373 axis->GetMutex();
374 // axis->SetValueNoMutex(0, 0,datas[2]/(float)X_AXIS_RANGE); //left stick
375 // x-axis
376 // axis->SetValueNoMutex(1, 0,datas[3]/(float)Y_AXIS_RANGE); //left stick
377 // y-axis
378 // axis->SetValueNoMutex(2, 0,datas[4]/(float)X_AXIS_RANGE); //right stick
379 // x-axis
380 // axis->SetValueNoMutex(3, 0,datas[5]/(float)Y_AXIS_RANGE); //right stick
381 // y-axis
382 axis->SetValueNoMutex(0, 0, datas[2]); // left stick x-axis
383 axis->SetValueNoMutex(1, 0, datas[3]); // left stick y-axis
384 axis->SetValueNoMutex(2, 0, datas[4]); // right stick x-axis
385 axis->SetValueNoMutex(3, 0, datas[5]); // right stick y-axis
386 axis->ReleaseMutex();
387 axis->SetDataTime(now);
388}
389
390void DualShock3::GetButtonData() {
391 // static uint8_t old_start_button=0;
392 button->GetMutex();
393 button->SetValueNoMutex(0, 0, (datas[0] & 0x08) == 0 ? 0 : 1); // start
394 /*
395 uint8_t start_button=datas[0]&0x08;
396 if (start_button!=old_start_button) {
397 if (start_button==0) {
398 Thread::Info("Debug: start button released\n");
399 } else {
400 Thread::Info("Debug: start button pressed\n");
401 }
402 old_start_button=start_button;
403 }
404 */
405 button->SetValueNoMutex(1, 0, (datas[0] & 0x01) == 0 ? 0 : 1); // select
406 button->SetValueNoMutex(2, 0, (datas[1] & 0x80) == 0 ? 0 : 1); // square
407 button->SetValueNoMutex(3, 0, (datas[1] & 0x10) == 0 ? 0 : 1); // triangle
408 button->SetValueNoMutex(4, 0, (datas[1] & 0x20) == 0 ? 0 : 1); // circle
409 button->SetValueNoMutex(5, 0, (datas[1] & 0x40) == 0 ? 0 : 1); // cross
410 button->SetValueNoMutex(6, 0, (datas[1] & 0x04) == 0 ? 0 : 1); // left 1
411 button->SetValueNoMutex(7, 0, (datas[1] & 0x01) == 0 ? 0 : 1); // left 2
412 button->SetValueNoMutex(8, 0, (datas[0] & 0x02) == 0 ? 0 : 1); // left 3
413 button->SetValueNoMutex(9, 0, (datas[1] & 0x08) == 0 ? 0 : 1); // right 1
414 button->SetValueNoMutex(10, 0, (datas[1] & 0x02) == 0 ? 0 : 1); // right 2
415 button->SetValueNoMutex(11, 0, (datas[0] & 0x04) == 0 ? 0 : 1); // right 3
416 button->SetValueNoMutex(12, 0, (datas[0] & 0x10) == 0 ? 0 : 1); // up
417 button->SetValueNoMutex(13, 0, (datas[0] & 0x40) == 0 ? 0 : 1); // down
418 button->SetValueNoMutex(14, 0, (datas[0] & 0x80) == 0 ? 0 : 1); // left
419 button->SetValueNoMutex(15, 0, (datas[0] & 0x20) == 0 ? 0 : 1); // right
420 button->ReleaseMutex();
421 button->SetDataTime(now);
422}
423
424void DualShock3::ProcessMessage(core::Message *controllerAction) {
425 ControllerAction action;
426 memcpy(&action, controllerAction->buffer, sizeof(ControllerAction));
427 if (action == ControllerAction::SetLedOn) {
428 Thread::Info("LedOn action request\n");
429 } else if (action == ControllerAction::SetLedOff) {
430 Thread::Info("LedOff action request\n");
431 } else if (action == ControllerAction::Rumble) {
432 Thread::Info("Rumble action request\n");
433 } else if (action == ControllerAction::FlashLed) {
434 Thread::Info("FlashLed action request\n");
435 }
436 // (char *msg, int msgSize)
437 /* for (unsigned int i=0; i<4; i++) {
438 if(msg[4+2*i]!=0 || msg[5+2*i]!=0) set_led(i+1,msg[4+2*i],msg[5+2*i]);
439
440 }
441 if(msg[0]!=0 || msg[2]!=0) rumble(msg[0],msg[1],msg[2],msg[3]);
442 */
443}
444
445// ----------------------------------------------------------------------
446// Replacement for libbluetooth
447
448int DualShock3::mystr2ba(const char *s, bdaddr_t *ba) {
449 if (strlen(s) != 17)
450 return 1;
451 for (int i = 0; i < 6; ++i) {
452 int d = strtol(s + 15 - 3 * i, NULL, 16);
453 if (d < 0 || d > 255)
454 return 1;
455 ba->b[i] = d;
456 }
457 return 0;
458}
459
460char *DualShock3::myba2str(const bdaddr_t *ba) {
461 static char buf[2][18]; // Static buffer valid for two invocations.
462 static int index = 0;
463 index = (index + 1) % 2;
464 sprintf(buf[index], "%02x:%02x:%02x:%02x:%02x:%02x", ba->b[5], ba->b[4],
465 ba->b[3], ba->b[2], ba->b[1], ba->b[0]);
466 return buf[index];
467}
468
469void DualShock3::fatal(const char *msg) {
470 if (errno)
471 perror(msg);
472 else
473 fprintf(stderr, "%s\n", msg);
474 exit(1);
475}
476
477/**********************************************************************/
478// Bluetooth HID devices
479// Incoming connections.
480
481int DualShock3::l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm) {
482 int sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
483 if (sk < 0)
484 fatal("socket");
485
486 struct sockaddr_l2 addr;
487 addr.l2_family = AF_BLUETOOTH;
488 addr.l2_bdaddr = *BDADDR_ANY;
489 addr.l2_psm = htobs(psm);
490 addr.l2_cid = 0;
491
492 if (bind(sk, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
493 close(sk);
494 fatal("bind");
495 }
496
497 if (listen(sk, 5) < 0)
498 fatal("listen");
499 return sk;
500}
501
502struct motion_dev *DualShock3::accept_device(int csk, int isk) {
503 Printf("Incoming connection...\n");
504 struct motion_dev *dev = (motion_dev *)malloc(sizeof(struct motion_dev));
505 if (!dev)
506 fatal("malloc");
507
508 dev->csk = accept(csk, NULL, NULL);
509 if (dev->csk < 0)
510 fatal("accept(CTRL)");
511 dev->isk = accept(isk, NULL, NULL);
512 if (dev->isk < 0)
513 fatal("accept(INTR)");
514
515 struct sockaddr_l2 addr;
516 socklen_t addrlen = sizeof(addr);
517 if (getpeername(dev->isk, (struct sockaddr *)&addr, &addrlen) < 0)
518 fatal("getpeername");
519 dev->addr = addr.l2_bdaddr;
520
521 // Distinguish SIXAXIS / DS3
522 unsigned char resp[64];
523 char get03f2[] = {HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_FEATURE | 8,
524 (char)0xf2, sizeof(resp), sizeof(resp) >> 8};
525 send(dev->csk, get03f2, sizeof(get03f2), 0); // 0301 is interesting too.
526 int nr = recv(dev->csk, resp, sizeof(resp), 0);
527
528 dev->type = (resp[13] == 0x40) ? SIXAXIS : DUALSHOCK3; // My guess.
529
530 return dev;
531}
532
533/**********************************************************************/
534// Device setup
535
536#define IR0 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x00, 0x41
537#define IR1 0x40, 0x00
538
539#define BIT1 2
540
541void DualShock3::hidp_trans(int csk, char *buf, int len) {
542 if (send(csk, buf, len, 0) != len)
543 fatal("send(CTRL)");
544 char ack;
545 int nr = recv(csk, &ack, sizeof(ack), 0);
546 if (nr != 1 || ack != 0)
547 fatal("ack");
548}
549
550void DualShock3::setup_device(struct motion_dev *dev) {
551
552 switch (dev->type) {
553 case SIXAXIS:
554 case DUALSHOCK3:
555 // Enable reporting
556 char set03f4[] = {HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE,
557 (char)0xf4, 0x42, 0x03, 0x00, 0x00};
558 hidp_trans(dev->csk, set03f4, sizeof(set03f4));
559
560 break;
561 }
562}
563
564/**********************************************************************/
565// Reports
566
567// faire un reglage station sol pour dead zone
568int8_t DualShock3::compute_dead_zone(int axis, unsigned char value) {
569 float tmp;
570
571 if (value > 128 + deadZone->Value()) {
572 tmp =
573 (value - deadZone->Value() - 128.) * 128. / (128. - deadZone->Value());
574 } else if (value < 128 - deadZone->Value()) {
575 // return value+DEAD_ZONE-128;
576 tmp =
577 (value + deadZone->Value() - 128.) * 127. / (128. - deadZone->Value());
578 } else {
579 return 0;
580 }
581
582 if (tmp > 127)
583 return 127;
584 if (tmp < -127)
585 return -127;
586 if (tmp > ((int8_t)tmp + .5) && tmp > 0)
587 return (int8_t)(tmp + 1);
588 if (tmp < ((int8_t)tmp - .5) && tmp < 0)
589 return (int8_t)(tmp - 1);
590
591 return (int8_t)tmp;
592}
593
594/**********************************************************************/
595// USB functions
596
597void DualShock3::usb_pair_device(struct usb_device *dev, int itfnum) {
598
599 usb_dev_handle *devh = usb_open(dev);
600 if (!devh)
601 fatal("usb_open");
602 usb_detach_kernel_driver_np(devh, itfnum);
603 int res = usb_claim_interface(devh, itfnum);
604 if (res < 0)
605 fatal("usb_claim_interface");
606
607 bdaddr_t current_ba; // Current pairing address.
608
609 switch (dev->descriptor.idProduct) {
610 case PRODUCT_SIXAXIS_DS3: {
611 // remote_printf("USB: SIXAXIS/DS3\n");
612 char msg[8];
613 res =
614 usb_control_msg(devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
615 USB_GET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
616 /*
617 unsigned char msg[8];
618 res = usb_control_msg
619 (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
620 USB_GET_REPORT, 0x03f5, itfnum, (void*)msg, sizeof(msg),
621 5000);*/
622 if (res < 0)
623 fatal("usb_control_msg(read master)");
624 for (int i = 0; i < 6; ++i)
625 current_ba.b[i] = (uint8_t)msg[7 - i];
626 break;
627 }
628 }
629
630 // New pairing address
631 int dev_id;
632 dev_id = hci_get_route(NULL);
633 struct hci_dev_info di;
634 hci_devinfo(dev_id, &di);
635
636 // Perform pairing.
637 if (!bacmp(&current_ba, &di.bdaddr)) {
638 printf(" Already paired to %s\n", myba2str(&di.bdaddr));
639 } else {
640 printf(" Changing master from %s to %s\n", myba2str(&current_ba),
641 myba2str(&di.bdaddr));
642 switch (dev->descriptor.idProduct) {
643 case PRODUCT_SIXAXIS_DS3: {
644 char msg[8] = {0x01, 0x00, (char)di.bdaddr.b[5], (char)di.bdaddr.b[4],
645 (char)di.bdaddr.b[3], (char)di.bdaddr.b[2],
646 (char)di.bdaddr.b[1], (char)di.bdaddr.b[0]};
647 res = usb_control_msg(
648 devh, USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
649 USB_SET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
650 if (res < 0)
651 fatal("usb_control_msg(write master)");
652 break;
653 }
654 }
655 }
656
657 if (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3)
658 printf(" Now unplug the USB cable and press the PS button.\n");
659 else
660 printf(" Now press the PS button.\n");
661}
662
663void DualShock3::usb_scan() {
664 usb_init();
665 if (usb_find_busses() < 0)
666 fatal("usb_find_busses");
667 if (usb_find_devices() < 0)
668 fatal("usb_find_devices");
669 struct usb_bus *busses = usb_get_busses();
670 if (!busses)
671 fatal("usb_get_busses");
672
673 struct usb_bus *bus;
674 for (bus = busses; bus; bus = bus->next) {
675 struct usb_device *dev;
676 for (dev = bus->devices; dev; dev = dev->next) {
677 struct usb_config_descriptor *cfg;
678 for (cfg = dev->config;
679 cfg < dev->config + dev->descriptor.bNumConfigurations; ++cfg) {
680 int itfnum;
681 for (itfnum = 0; itfnum < cfg->bNumInterfaces; ++itfnum) {
682 struct usb_interface *itf = &cfg->interface[itfnum];
683 struct usb_interface_descriptor *alt;
684 for (alt = itf->altsetting;
685 alt < itf->altsetting + itf->num_altsetting; ++alt) {
686 if (dev->descriptor.idVendor == VENDOR_SONY &&
687 (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3) &&
688 alt->bInterfaceClass == 3)
689 usb_pair_device(dev, itfnum);
690 }
691 }
692 }
693 }
694 }
695}
696
697void DualShock3::rumble(uint8_t left_force, uint8_t left_timeout,
698 uint8_t right_force, uint8_t right_timeout) {
699 // printf("rumble\n");
700
701 unsigned char datas[] = {
702 0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */, 0x01, 0x00, 0x00,
703 0x00, 0x00, 0x00, // rumble values
704 0x00, 0x00, 0x00, 0x00, (unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4
705
706 0xff, 0x27, led4_on, led4_off, 0x32, 0xff, 0x27, led3_on, led3_off, 0x32,
707 0xff, 0x27, led2_on, led2_off, 0x32, 0xff, 0x27, led1_on, led1_off, 0x32,
708 0x00, 0x00, 0x00, 0x00, 0x00,
709 };
710
711 datas[5] = left_timeout; // left timeout
712 datas[6] = left_force; // left force
713 datas[3] = right_timeout; // right timeout
714 datas[4] = right_force; // right force
715
716 if (connectionType == Bluetooth) {
717 hidp_trans(dev->csk, (char *)datas, sizeof(datas));
718 }
719}
720
721void DualShock3::set_led(uint8_t led, uint8_t on_timeout, uint8_t off_timeout) {
722 uint8_t mask;
723
724 switch (led) {
725 case 1:
726 led1_on = on_timeout;
727 led1_off = off_timeout;
728 mask = 2;
729 break;
730 case 2:
731 led2_on = on_timeout;
732 led2_off = off_timeout;
733 mask = 4;
734 break;
735 case 3:
736 led3_on = on_timeout;
737 led3_off = off_timeout;
738 mask = 8;
739 break;
740 case 4:
741 led4_on = on_timeout;
742 led4_off = off_timeout;
743 mask = 16;
744 break;
745 }
746
747 if (on_timeout != 0) {
748 ledmask |= mask;
749 } else {
750 ledmask &= ~mask;
751 }
752 /*
753 printf("led %x\n",ledmask);
754 printf("1:%i %i\n",led1_on,led1_off);
755 printf("2:%i %i\n",led2_on,led2_off);
756 printf("3:%i %i\n",led3_on,led3_off);
757 printf("4:%i %i\n",led4_on,led4_off);*/
758
759 unsigned char datas[] = {
760 0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */, 0x01, 0x00, 0x00,
761 0x00, 0x00, 0x00, // rumble values
762 0x00, 0x00, 0x00, 0x00, (unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4
763
764 0xff, 0x27, led4_on, led4_off, 0x32, 0xff, 0x27, led3_on, led3_off, 0x32,
765 0xff, 0x27, led2_on, led2_off, 0x32, 0xff, 0x27, led1_on, led1_off, 0x32,
766 0x00, 0x00, 0x00, 0x00, 0x00,
767 };
768
769 if (connectionType == Bluetooth) {
770 hidp_trans(dev->csk, (char *)datas, sizeof(datas));
771 }
772}
773
774} // end namespace sensor
775} // end namespace flair
Note: See TracBrowser for help on using the repository browser.