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

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