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

Last change on this file since 14 was 11, checked in by Sanahuja Guillaume, 9 years ago

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