Changeset 16 in flair-src for trunk/tools/Controller/DualShock3/src


Ignore:
Timestamp:
Apr 8, 2016, 3:48:40 PM (9 years ago)
Author:
Bayard Gildas
Message:

Reformatting

Location:
trunk/tools/Controller/DualShock3/src
Files:
3 edited

Legend:

Unmodified
Added
Removed

  • trunk/tools/Controller/DualShock3/src/DualShock3.cpp

    r11 r16  
    1111//  version:    $Id: $
    1212//
    13 //  purpose:    Base class for host side remote controls that talks to target side through ethernet connection
     13//  purpose:    Base class for host side remote controls that talks to target
     14//  side through ethernet connection
    1415//
    1516//
    … …  
    3940#include <bluetooth/hci_lib.h>
    4041
    41 
    4242#define USB_DIR_IN 0x80
    4343#define USB_DIR_OUT 0
    … …  
    5050#define L2CAP_PSM_HIDP_INTR 0x13
    5151
    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
     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
    5656
    5757#define SIXAXIS 1
    … …  
    6666
    6767typedef struct motion_dev {
    68     int index;
    69     bdaddr_t addr;
    70     char type;
    71     int csk;
    72     int isk;
    73     struct motion_dev *next;
     68  int index;
     69  bdaddr_t addr;
     70  char type;
     71  int csk;
     72  int isk;
     73  struct motion_dev *next;
    7474} motion_dev_t;
    7575
    76 bdaddr_t bdaddr_any={{0,0,0,0,0,0}};
    77 
    78 namespace flair
    79 {
    80 namespace sensor
    81 {
    82 
    83 DualShock3::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);
     76bdaddr_t bdaddr_any = {{0, 0, 0, 0, 0, 0}};
     77
     78namespace flair {
     79namespace sensor {
     80
     81DualShock3::DualShock3(const FrameworkManager *parent, string name,
     82                       string receiverAddress, int receiverPort,
     83                       ConnectionType_t _connectionType, uint32_t period,
     84                       uint32_t bitsPerAxis, uint8_t priority)
     85    : HostEthController(parent, 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          }
    141171        }
    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             }
     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);
    167322        }
    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 
    185 DualShock3::~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 
    195 string 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 
    215 string 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 
    252 bool 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);
     323      }
     324      if (report[0] == 0xa1) {
     325        return parse_report_sixaxis_ds3(report + 1, nr - 1);
     326      }
    308327    }
    309328    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;
    310335}
    311336
    312337bool 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;
     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;
    336361}
    337362
    338363void DualShock3::GetAxisData() {
    339364
    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);
     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);
    351380}
    352381
    353382void 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);
     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);
    385414}
    386415
    387416void 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         */
     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          */
    406435}
    407436
    … …  
    410439
    411440int 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;
     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;
    419450}
    420451
    421452char *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];
     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];
    428459}
    429460
    430461void DualShock3::fatal(const char *msg) {
    431     if ( errno ) perror(msg);
    432     else fprintf(stderr, "%s\n", msg);
    433     exit(1);
     462  if (errno)
     463    perror(msg);
     464  else
     465    fprintf(stderr, "%s\n", msg);
     466  exit(1);
    434467}
    435468
    … …  
    439472
    440473int 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;
     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;
    458492}
    459493
    460494struct 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;
     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;
    486523}
    487524
    … …  
    495532
    496533void 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");
     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");
    501540}
    502541
    503542void DualShock3::setup_device(struct motion_dev *dev) {
    504543
    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     }
     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  }
    516554}
    517555
    … …  
    519557// Reports
    520558
    521 //faire un reglage station sol pour dead zone
    522 int8_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;
     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;
    541584}
    542585
    … …  
    544587// USB functions
    545588
    546 void 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 ) {
     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) {
    558635    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;
     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          }
    602683        }
    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 
    612 void 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 
    652 void 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 
    681 void 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     }
     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  }
    737764}
    738765

  • trunk/tools/Controller/DualShock3/src/DualShock3.h

    r11 r16  
    1212//  version:    $Id: $
    1313//
    14 //  purpose:    Sony DualShock3 host side driver class. Talks to target side through ethernet.
     14//  purpose:    Sony DualShock3 host side driver class. Talks to target side
     15//  through ethernet.
    1516//
    1617//
    … …  
    2829
    2930namespace flair {
    30     namespace core {
    31         class FrameworkManager;
    32     }
    33     namespace gui {
    34         class SpinBox;
    35         class Label;
    36         class CheckBox;
    37     }
     31namespace core {
     32class FrameworkManager;
     33}
     34namespace gui {
     35class SpinBox;
     36class Label;
     37class CheckBox;
     38}
    3839}
    3940
    4041struct motion_dev;
    4142
    42 namespace flair { namespace sensor {
    43     /*! \class DualShock3
    44     *
    45     * \brief Sony DualShock3 host side driver class. Talks to target side through ethernet.
    46     */
    47     class DualShock3 : public HostEthController {
    48     public:
    49         typedef enum {
    50             Usb,
    51             Bluetooth
    52         } ConnectionType_t;
     43namespace flair {
     44namespace sensor {
     45/*! \class DualShock3
     46*
     47* \brief Sony DualShock3 host side driver class. Talks to target side through
     48*ethernet.
     49*/
     50class DualShock3 : public HostEthController {
     51public:
     52  typedef enum { Usb, Bluetooth } ConnectionType_t;
    5353
    54         DualShock3(const core::FrameworkManager* parent,std::string name,std::string receiverAddress,int receiverPort,ConnectionType_t connectionType,uint32_t period=10,uint32_t bitsPerAxis=7,uint8_t priority=0);
    55         ~DualShock3();
    56     private:
    57         gui::SpinBox* deadZone;
    58         gui::CheckBox* enabled;
    59         gui::Label *batteryChargeLevel;
    60         ConnectionType_t connectionType;
    61         core::Time now;
     54  DualShock3(const core::FrameworkManager *parent, std::string name,
     55             std::string receiverAddress, int receiverPort,
     56             ConnectionType_t connectionType, uint32_t period = 10,
     57             uint32_t bitsPerAxis = 7, uint8_t priority = 0);
     58  ~DualShock3();
    6259
    63         std::string GetAxisDescription(unsigned int axis);
    64         std::string GetButtonDescription(unsigned int button);
    65         void GetAxisData();
    66         void GetButtonData();
    67         bool IsDataFrameReady();
    68         void ProcessMessage(core::Message *controllerAction);
     60private:
     61  gui::SpinBox *deadZone;
     62  gui::CheckBox *enabled;
     63  gui::Label *batteryChargeLevel;
     64  ConnectionType_t connectionType;
     65  core::Time now;
    6966
    70         void UpdateFrom(const core::io_data *data) {};
    71         void fatal(const char *msg);
    72         int l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm);
    73         struct motion_dev *accept_device(int csk, int isk);
    74         void hidp_trans(int csk, char *buf, int len);
    75         void setup_device(struct motion_dev *dev);
    76         bool parse_report_sixaxis_ds3(unsigned char *r, int len);
    77         int mystr2ba(const char *s, bdaddr_t *ba);
    78         char *myba2str(const bdaddr_t *ba);
    79         int8_t compute_dead_zone(int axis,unsigned char value);
    80         struct motion_dev *dev;
    81         int usb_fd;
    82         int isk;
    83         core::Time last_voltage_time;
     67  std::string GetAxisDescription(unsigned int axis);
     68  std::string GetButtonDescription(unsigned int button);
     69  void GetAxisData();
     70  void GetButtonData();
     71  bool IsDataFrameReady();
     72  void ProcessMessage(core::Message *controllerAction);
    8473
    85         int8_t *datas;
    86         uint8_t dataSize;
     74  void UpdateFrom(const core::io_data *data){};
     75  void fatal(const char *msg);
     76  int l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm);
     77  struct motion_dev *accept_device(int csk, int isk);
     78  void hidp_trans(int csk, char *buf, int len);
     79  void setup_device(struct motion_dev *dev);
     80  bool parse_report_sixaxis_ds3(unsigned char *r, int len);
     81  int mystr2ba(const char *s, bdaddr_t *ba);
     82  char *myba2str(const bdaddr_t *ba);
     83  int8_t compute_dead_zone(int axis, unsigned char value);
     84  struct motion_dev *dev;
     85  int usb_fd;
     86  int isk;
     87  core::Time last_voltage_time;
    8788
    88         void usb_scan();
    89         void usb_pair_device(struct usb_device *dev, int itfnum);
     89  int8_t *datas;
     90  uint8_t dataSize;
    9091
    91         void rumble(uint8_t left_force,uint8_t left_timeout,uint8_t right_force,uint8_t right_timeout);
    92         void set_led(uint8_t led,uint8_t on_timeout,uint8_t off_timeout);
    93         char ledmask;
    94         uint8_t led1_on,led1_off,led2_on,led2_off,led3_on,led3_off,led4_on,led4_off;
     92  void usb_scan();
     93  void usb_pair_device(struct usb_device *dev, int itfnum);
    9594
    96     };
    97 }}
     95  void rumble(uint8_t left_force, uint8_t left_timeout, uint8_t right_force,
     96              uint8_t right_timeout);
     97  void set_led(uint8_t led, uint8_t on_timeout, uint8_t off_timeout);
     98  char ledmask;
     99  uint8_t led1_on, led1_off, led2_on, led2_off, led3_on, led3_off, led4_on,
     100      led4_off;
     101};
     102}
     103}
    98104
    99105#endif // DUALSHOCK3_H

  • trunk/tools/Controller/DualShock3/src/main.cpp

    r11 r16  
    3333string xml_file;
    3434
    35 void parseOptions(int argc, char** argv);
     35void parseOptions(int argc, char **argv);
    3636
    37 int main(int argc, char* argv[]) {
    38     parseOptions(argc,argv);
     37int main(int argc, char *argv[]) {
     38  parseOptions(argc, argv);
    3939
    40     DualShock3 *joystick;
    41     FrameworkManager *manager;
     40  DualShock3 *joystick;
     41  FrameworkManager *manager;
    4242
    43     manager= new FrameworkManager("dualshock3");
    44     manager->SetupConnection("127.0.0.1",port);
    45     manager->SetupUserInterface(xml_file);
     43  manager = new FrameworkManager("dualshock3");
     44  manager->SetupConnection("127.0.0.1", port);
     45  manager->SetupUserInterface(xml_file);
    4646
    47     if(connection=="usb") {
    48 //        manager->Warn("!! adresse = %s !!\n", address.c_str());
    49         joystick=new DualShock3(manager,"dualshock3",receiverAddress,receiverPort,DualShock3::Usb,period,6);
    50     } else {
    51         joystick=new DualShock3(manager,"dualshock3",receiverAddress,receiverPort,DualShock3::Bluetooth,period,6);
    52     }
     47  if (connection == "usb") {
     48    //        manager->Warn("!! adresse = %s !!\n", address.c_str());
     49    joystick = new DualShock3(manager, "dualshock3", receiverAddress,
     50                              receiverPort, DualShock3::Usb, period, 6);
     51  } else {
     52    joystick = new DualShock3(manager, "dualshock3", receiverAddress,
     53                              receiverPort, DualShock3::Bluetooth, period, 6);
     54  }
    5355
    54     joystick->DrawUserInterface();
     56  joystick->DrawUserInterface();
    5557
    56     if(!manager->ErrorOccured()) {
    57         joystick->Start();
    58         joystick->Join();
    59     }
     58  if (!manager->ErrorOccured()) {
     59    joystick->Start();
     60    joystick->Join();
     61  }
    6062
    61     delete manager;
     63  delete manager;
    6264}
    6365
    64 void parseOptions(int argc, char** argv) {
    65     try {
    66         CmdLine cmd("Command description message", ' ', "0.1");
     66void parseOptions(int argc, char **argv) {
     67  try {
     68    CmdLine cmd("Command description message", ' ', "0.1");
    6769
    68         ValueArg<string> addressArg("a","address","data receiver address (ex: uav)",true,"127.0.0.1:20000","string");
    69         cmd.add(addressArg);
     70    ValueArg<string> addressArg("a", "address",
     71                                "data receiver address (ex: uav)", true,
     72                                "127.0.0.1:20000", "string");
     73    cmd.add(addressArg);
    7074
    71         ValueArg<string> connectionArg("c","connection","connection type (usb or bluetooth)",false,"bluetooth","string");
    72         cmd.add(connectionArg);
     75    ValueArg<string> connectionArg("c", "connection",
     76                                   "connection type (usb or bluetooth)", false,
     77                                   "bluetooth", "string");
     78    cmd.add(connectionArg);
    7379
    74         ValueArg<int> portArg("p","port","local port used to connect to the ground station",false,9000,"int");
    75         cmd.add(portArg);
     80    ValueArg<int> portArg("p", "port",
     81                          "local port used to connect to the ground station",
     82                          false, 9000, "int");
     83    cmd.add(portArg);
    7684
    77         ValueArg<int> periodArg("t","period","sending data period",false,10,"int");
    78         cmd.add(periodArg);
     85    ValueArg<int> periodArg("t", "period", "sending data period", false, 10,
     86                            "int");
     87    cmd.add(periodArg);
    7988
    80         ValueArg<string> xmlArg("x","xml","xml file",true,"./settings.xml","string");
    81         cmd.add( xmlArg );
     89    ValueArg<string> xmlArg("x", "xml", "xml file", true, "./settings.xml",
     90                            "string");
     91    cmd.add(xmlArg);
    8292
    83         cmd.parse(argc, argv);
     93    cmd.parse(argc, argv);
    8494
    85         // Get the value parsed by each arg.
    86         string receiverAddressWithPort=addressArg.getValue();
    87         int semiColonPosition = receiverAddressWithPort.find(":");
    88         receiverAddress=receiverAddressWithPort.substr (0,semiColonPosition);
    89         receiverPort=atoi(receiverAddressWithPort.substr(semiColonPosition+1).c_str());
    90         connection=connectionArg.getValue();
    91         port=portArg.getValue();
    92         period=periodArg.getValue();
    93         xml_file=xmlArg.getValue();
     95    // Get the value parsed by each arg.
     96    string receiverAddressWithPort = addressArg.getValue();
     97    int semiColonPosition = receiverAddressWithPort.find(":");
     98    receiverAddress = receiverAddressWithPort.substr(0, semiColonPosition);
     99    receiverPort =
     100        atoi(receiverAddressWithPort.substr(semiColonPosition + 1).c_str());
     101    connection = connectionArg.getValue();
     102    port = portArg.getValue();
     103    period = periodArg.getValue();
     104    xml_file = xmlArg.getValue();
    94105
    95     } catch (ArgException &e) {  // catch any exceptions
    96         cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
    97     }
     106  } catch (ArgException &e) { // catch any exceptions
     107    cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
     108  }
    98109}
Note: See TracChangeset for help on using the changeset viewer.