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

Last change on this file since 304 was 300, checked in by Sanahuja Guillaume, 4 years ago

m

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