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

Last change on this file since 88 was 16, checked in by Bayard Gildas, 9 years ago

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