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

Last change on this file since 11 was 11, checked in by Sanahuja Guillaume, 5 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.