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

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

singleton manager

File size: 21.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
14//  side through ethernet connection
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
52#define HIDP_TRANS_GET_REPORT 0x40
53#define HIDP_TRANS_SET_REPORT 0x50
54#define HIDP_DATA_RTYPE_OUTPUT 0x02
55#define HIDP_DATA_RTYPE_FEATURE 0x03
56
57#define SIXAXIS 1
58#define DUALSHOCK3 2
59
60#define X_AXIS_RANGE 127
61#define Y_AXIS_RANGE 127
62
63using namespace flair::core;
64using namespace flair::gui;
65using namespace std;
66
67typedef struct motion_dev {
68  int index;
69  bdaddr_t addr;
70  char type;
71  int csk;
72  int isk;
73  struct motion_dev *next;
74} motion_dev_t;
75
76bdaddr_t bdaddr_any = {{0, 0, 0, 0, 0, 0}};
77
78namespace flair {
79namespace sensor {
80
81DualShock3::DualShock3(string name,
82                       string receiverAddress, int receiverPort,
83                       ConnectionType_t _connectionType, uint32_t period,
84                       uint32_t bitsPerAxis, uint8_t priority)
85    : HostEthController(name, receiverAddress, receiverPort, period,
86                        bitsPerAxis, priority),
87      connectionType(_connectionType) {
88  controllerName = "DualShock3";
89  last_voltage_time = 0;
90
91  // axis stuff
92  axisNumber = 4;
93  nativeBitsPerAxis = 8;
94  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);
100
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);
110
111  Tab *settingsTab = new Tab(tabWidget, "Settings");
112  dataSize = 6;
113  datas = new int8_t[dataSize];
114
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");
122
123  if (connectionType == Bluetooth) {
124    // init DS3
125    usb_scan();
126
127    int csk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_CTRL);
128    isk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_INTR);
129
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");
134
135    fd_set fds;
136    FD_ZERO(&fds);
137
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 ?
143
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];
152
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;
159
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          }
171        }
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    }
179
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    }
184
185    if (nr < 49 || nr > 50) {
186      Thread::Err("sixad-raw::read(fd) - not a sixaxis (nr = %i )\n", nr);
187    }
188  }
189  ledmask = 0;
190}
191
192DualShock3::~DualShock3() {
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  }
200}
201
202string DualShock3::GetAxisDescription(unsigned int axis) {
203  string description;
204
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;
220}
221
222string DualShock3::GetButtonDescription(unsigned int button) {
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  }
273}
274
275bool DualShock3::IsDataFrameReady() {
276  unsigned char report[256];
277  unsigned char tmp_report[256];
278
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;
291*/
292    FD_SET(dev->isk, &fds);
293    if (dev->isk > fdmax)
294      fdmax = dev->isk;
295
296    if (select(fdmax + 1, &fds, NULL, NULL, NULL) < 0)
297      fatal("select");
298
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);
322        }
323      }
324      if (report[0] == 0xa1) {
325        return parse_report_sixaxis_ds3(report + 1, nr - 1);
326      }
327    }
328    return false;
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;
335}
336
337bool DualShock3::parse_report_sixaxis_ds3(unsigned char *r, int len) {
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]);
345
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    }
357
358    return true;
359  }
360  return false;
361}
362
363void DualShock3::GetAxisData() {
364
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);
380}
381
382void DualShock3::GetButtonData() {
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);
414}
415
416void DualShock3::ProcessMessage(core::Message *controllerAction) {
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]);
431
432      }
433      if(msg[0]!=0 || msg[2]!=0) rumble(msg[0],msg[1],msg[2],msg[3]);
434          */
435}
436
437// ----------------------------------------------------------------------
438// Replacement for libbluetooth
439
440int DualShock3::mystr2ba(const char *s, bdaddr_t *ba) {
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;
450}
451
452char *DualShock3::myba2str(const bdaddr_t *ba) {
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];
459}
460
461void DualShock3::fatal(const char *msg) {
462  if (errno)
463    perror(msg);
464  else
465    fprintf(stderr, "%s\n", msg);
466  exit(1);
467}
468
469/**********************************************************************/
470// Bluetooth HID devices
471// Incoming connections.
472
473int DualShock3::l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm) {
474  int sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
475  if (sk < 0)
476    fatal("socket");
477
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;
483
484  if (bind(sk, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
485    close(sk);
486    fatal("bind");
487  }
488
489  if (listen(sk, 5) < 0)
490    fatal("listen");
491  return sk;
492}
493
494struct motion_dev *DualShock3::accept_device(int csk, int isk) {
495  Printf("Incoming connection...\n");
496  struct motion_dev *dev = (motion_dev *)malloc(sizeof(struct motion_dev));
497  if (!dev)
498    fatal("malloc");
499
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)");
506
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;
512
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);
519
520  dev->type = (resp[13] == 0x40) ? SIXAXIS : DUALSHOCK3; // My guess.
521
522  return dev;
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) {
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");
540}
541
542void DualShock3::setup_device(struct motion_dev *dev) {
543
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));
551
552    break;
553  }
554}
555
556/**********************************************************************/
557// Reports
558
559// faire un reglage station sol pour dead zone
560int8_t DualShock3::compute_dead_zone(int axis, unsigned char value) {
561  float tmp;
562
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  }
573
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);
582
583  return (int8_t)tmp;
584}
585
586/**********************************************************************/
587// USB functions
588
589void DualShock3::usb_pair_device(struct usb_device *dev, int itfnum) {
590
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");
598
599  bdaddr_t current_ba; // Current pairing address.
600
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  }
621
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);
627
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;
645    }
646    }
647  }
648
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");
653}
654
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");
664
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          }
683        }
684      }
685    }
686  }
687}
688
689void DualShock3::rumble(uint8_t left_force, uint8_t left_timeout,
690                        uint8_t right_force, uint8_t right_timeout) {
691  // printf("rumble\n");
692
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
697
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  };
702
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
707
708  if (connectionType == Bluetooth) {
709    hidp_trans(dev->csk, (char *)datas, sizeof(datas));
710  }
711}
712
713void DualShock3::set_led(uint8_t led, uint8_t on_timeout, uint8_t off_timeout) {
714  uint8_t mask;
715
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  }
738
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);*/
750
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
755
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  };
760
761  if (connectionType == Bluetooth) {
762    hidp_trans(dev->csk, (char *)datas, sizeof(datas));
763  }
764}
765
766} // end namespace sensor
767} // end namespace flair
Note: See TracBrowser for help on using the repository browser.