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

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

m

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