source: flair-src/branches/sanscv/tools/Controller/DualShock3/src/DualShock3.cpp@ 377

Last change on this file since 377 was 324, checked in by Sanahuja Guillaume, 5 years ago

removing opencv dependency
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 <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 MatrixDescriptor *axisDescriptor = new MatrixDescriptor(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 MatrixDescriptor *buttonDescriptor =
105 new MatrixDescriptor(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 char errorMsg[256];
494 Thread::Err("bth bind, %s\n", strerror_r(errno, errorMsg, sizeof(errorMsg)));
495 return -1;
496 }
497
498 if (listen(sk, 5) < 0)
499 fatal("listen");
500 return sk;
501}
502
503struct motion_dev *DualShock3::accept_device(int csk, int isk) {
504 Printf("Incoming connection...\n");
505 struct motion_dev *dev = (motion_dev *)malloc(sizeof(struct motion_dev));
506 if (!dev)
507 fatal("malloc");
508
509 dev->csk = accept(csk, NULL, NULL);
510 if (dev->csk < 0)
511 fatal("accept(CTRL)");
512 dev->isk = accept(isk, NULL, NULL);
513 if (dev->isk < 0)
514 fatal("accept(INTR)");
515
516 struct sockaddr_l2 addr;
517 socklen_t addrlen = sizeof(addr);
518 if (getpeername(dev->isk, (struct sockaddr *)&addr, &addrlen) < 0)
519 fatal("getpeername");
520 dev->addr = addr.l2_bdaddr;
521
522 // Distinguish SIXAXIS / DS3
523 unsigned char resp[64];
524 char get03f2[] = {HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_FEATURE | 8,
525 (char)0xf2, sizeof(resp), sizeof(resp) >> 8};
526 send(dev->csk, get03f2, sizeof(get03f2), 0); // 0301 is interesting too.
527 int nr = recv(dev->csk, resp, sizeof(resp), 0);
528
529 dev->type = (resp[13] == 0x40) ? SIXAXIS : DUALSHOCK3; // My guess.
530
531 return dev;
532}
533
534/**********************************************************************/
535// Device setup
536
537#define IR0 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x00, 0x41
538#define IR1 0x40, 0x00
539
540#define BIT1 2
541
542void DualShock3::hidp_trans(int csk, char *buf, int len) {
543 if (send(csk, buf, len, 0) != len)
544 fatal("send(CTRL)");
545 char ack;
546 int nr = recv(csk, &ack, sizeof(ack), 0);
547 if (nr != 1 || ack != 0)
548 fatal("ack");
549}
550
551void DualShock3::setup_device(struct motion_dev *dev) {
552
553 switch (dev->type) {
554 case SIXAXIS:
555 case DUALSHOCK3:
556 // Enable reporting
557 char set03f4[] = {HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE,
558 (char)0xf4, 0x42, 0x03, 0x00, 0x00};
559 hidp_trans(dev->csk, set03f4, sizeof(set03f4));
560
561 break;
562 }
563}
564
565/**********************************************************************/
566// Reports
567
568// faire un reglage station sol pour dead zone
569int8_t DualShock3::compute_dead_zone(int axis, unsigned char value) {
570 float tmp;
571
572 if (value > 128 + deadZone->Value()) {
573 tmp =
574 (value - deadZone->Value() - 128.) * 128. / (128. - deadZone->Value());
575 } else if (value < 128 - deadZone->Value()) {
576 // return value+DEAD_ZONE-128;
577 tmp =
578 (value + deadZone->Value() - 128.) * 127. / (128. - deadZone->Value());
579 } else {
580 return 0;
581 }
582
583 if (tmp > 127)
584 return 127;
585 if (tmp < -127)
586 return -127;
587 if (tmp > ((int8_t)tmp + .5) && tmp > 0)
588 return (int8_t)(tmp + 1);
589 if (tmp < ((int8_t)tmp - .5) && tmp < 0)
590 return (int8_t)(tmp - 1);
591
592 return (int8_t)tmp;
593}
594
595/**********************************************************************/
596// USB functions
597
598void DualShock3::usb_pair_device(struct usb_device *dev, int itfnum) {
599
600 usb_dev_handle *devh = usb_open(dev);
601 if (!devh)
602 fatal("usb_open");
603 usb_detach_kernel_driver_np(devh, itfnum);
604 int res = usb_claim_interface(devh, itfnum);
605 if (res < 0)
606 fatal("usb_claim_interface");
607
608 bdaddr_t current_ba; // Current pairing address.
609
610 switch (dev->descriptor.idProduct) {
611 case PRODUCT_SIXAXIS_DS3: {
612 // remote_printf("USB: SIXAXIS/DS3\n");
613 char msg[8];
614 res =
615 usb_control_msg(devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
616 USB_GET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
617 /*
618 unsigned char msg[8];
619 res = usb_control_msg
620 (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
621 USB_GET_REPORT, 0x03f5, itfnum, (void*)msg, sizeof(msg),
622 5000);*/
623 if (res < 0)
624 fatal("usb_control_msg(read master)");
625 for (int i = 0; i < 6; ++i)
626 current_ba.b[i] = (uint8_t)msg[7 - i];
627 break;
628 }
629 }
630
631 // New pairing address
632 int dev_id;
633 dev_id = hci_get_route(NULL);
634 struct hci_dev_info di;
635 hci_devinfo(dev_id, &di);
636
637 // Perform pairing.
638 if (!bacmp(&current_ba, &di.bdaddr)) {
639 printf(" Already paired to %s\n", myba2str(&di.bdaddr));
640 } else {
641 printf(" Changing master from %s to %s\n", myba2str(&current_ba),
642 myba2str(&di.bdaddr));
643 switch (dev->descriptor.idProduct) {
644 case PRODUCT_SIXAXIS_DS3: {
645 char msg[8] = {0x01, 0x00, (char)di.bdaddr.b[5], (char)di.bdaddr.b[4],
646 (char)di.bdaddr.b[3], (char)di.bdaddr.b[2],
647 (char)di.bdaddr.b[1], (char)di.bdaddr.b[0]};
648 res = usb_control_msg(
649 devh, USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
650 USB_SET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
651 if (res < 0)
652 fatal("usb_control_msg(write master)");
653 break;
654 }
655 }
656 }
657
658 if (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3)
659 printf(" Now unplug the USB cable and press the PS button.\n");
660 else
661 printf(" Now press the PS button.\n");
662}
663
664void DualShock3::usb_scan() {
665 usb_init();
666 if (usb_find_busses() < 0)
667 fatal("usb_find_busses");
668 if (usb_find_devices() < 0)
669 fatal("usb_find_devices");
670 struct usb_bus *busses = usb_get_busses();
671 if (!busses)
672 fatal("usb_get_busses");
673
674 struct usb_bus *bus;
675 for (bus = busses; bus; bus = bus->next) {
676 struct usb_device *dev;
677 for (dev = bus->devices; dev; dev = dev->next) {
678 struct usb_config_descriptor *cfg;
679 for (cfg = dev->config;
680 cfg < dev->config + dev->descriptor.bNumConfigurations; ++cfg) {
681 int itfnum;
682 for (itfnum = 0; itfnum < cfg->bNumInterfaces; ++itfnum) {
683 struct usb_interface *itf = &cfg->interface[itfnum];
684 struct usb_interface_descriptor *alt;
685 for (alt = itf->altsetting;
686 alt < itf->altsetting + itf->num_altsetting; ++alt) {
687 if (dev->descriptor.idVendor == VENDOR_SONY &&
688 (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3) &&
689 alt->bInterfaceClass == 3)
690 usb_pair_device(dev, itfnum);
691 }
692 }
693 }
694 }
695 }
696}
697
698void DualShock3::rumble(uint8_t left_force, uint8_t left_timeout,
699 uint8_t right_force, uint8_t right_timeout) {
700 // printf("rumble\n");
701
702 unsigned char datas[] = {
703 0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */, 0x01, 0x00, 0x00,
704 0x00, 0x00, 0x00, // rumble values
705 0x00, 0x00, 0x00, 0x00, (unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4
706
707 0xff, 0x27, led4_on, led4_off, 0x32, 0xff, 0x27, led3_on, led3_off, 0x32,
708 0xff, 0x27, led2_on, led2_off, 0x32, 0xff, 0x27, led1_on, led1_off, 0x32,
709 0x00, 0x00, 0x00, 0x00, 0x00,
710 };
711
712 datas[5] = left_timeout; // left timeout
713 datas[6] = left_force; // left force
714 datas[3] = right_timeout; // right timeout
715 datas[4] = right_force; // right force
716
717 if (connectionType == Bluetooth) {
718 hidp_trans(dev->csk, (char *)datas, sizeof(datas));
719 }
720}
721
722void DualShock3::set_led(uint8_t led, uint8_t on_timeout, uint8_t off_timeout) {
723 uint8_t mask;
724
725 switch (led) {
726 case 1:
727 led1_on = on_timeout;
728 led1_off = off_timeout;
729 mask = 2;
730 break;
731 case 2:
732 led2_on = on_timeout;
733 led2_off = off_timeout;
734 mask = 4;
735 break;
736 case 3:
737 led3_on = on_timeout;
738 led3_off = off_timeout;
739 mask = 8;
740 break;
741 case 4:
742 led4_on = on_timeout;
743 led4_off = off_timeout;
744 mask = 16;
745 break;
746 }
747
748 if (on_timeout != 0) {
749 ledmask |= mask;
750 } else {
751 ledmask &= ~mask;
752 }
753 /*
754 printf("led %x\n",ledmask);
755 printf("1:%i %i\n",led1_on,led1_off);
756 printf("2:%i %i\n",led2_on,led2_off);
757 printf("3:%i %i\n",led3_on,led3_off);
758 printf("4:%i %i\n",led4_on,led4_off);*/
759
760 unsigned char datas[] = {
761 0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */, 0x01, 0x00, 0x00,
762 0x00, 0x00, 0x00, // rumble values
763 0x00, 0x00, 0x00, 0x00, (unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4
764
765 0xff, 0x27, led4_on, led4_off, 0x32, 0xff, 0x27, led3_on, led3_off, 0x32,
766 0xff, 0x27, led2_on, led2_off, 0x32, 0xff, 0x27, led1_on, led1_off, 0x32,
767 0x00, 0x00, 0x00, 0x00, 0x00,
768 };
769
770 if (connectionType == Bluetooth) {
771 hidp_trans(dev->csk, (char *)datas, sizeof(datas));
772 }
773}
774
775} // end namespace sensor
776} // end namespace flair
Note: See TracBrowser for help on using the repository browser.