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

Last change on this file since 168 was 148, checked in by Sanahuja Guillaume, 8 years ago

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