Index: trunk/tools/Controller/DualShock3/src/DualShock3.cpp
===================================================================
--- trunk/tools/Controller/DualShock3/src/DualShock3.cpp	(revision 11)
+++ trunk/tools/Controller/DualShock3/src/DualShock3.cpp	(revision 16)
@@ -11,5 +11,6 @@
 //  version:    $Id: $
 //
-//  purpose:    Base class for host side remote controls that talks to target side through ethernet connection
+//  purpose:    Base class for host side remote controls that talks to target
+//  side through ethernet connection
 //
 //
@@ -39,5 +40,4 @@
 #include <bluetooth/hci_lib.h>
 
-
 #define USB_DIR_IN 0x80
 #define USB_DIR_OUT 0
@@ -50,8 +50,8 @@
 #define L2CAP_PSM_HIDP_INTR 0x13
 
-#define HIDP_TRANS_GET_REPORT    0x40
-#define HIDP_TRANS_SET_REPORT    0x50
-#define HIDP_DATA_RTYPE_OUTPUT   0x02
-#define HIDP_DATA_RTYPE_FEATURE  0x03
+#define HIDP_TRANS_GET_REPORT 0x40
+#define HIDP_TRANS_SET_REPORT 0x50
+#define HIDP_DATA_RTYPE_OUTPUT 0x02
+#define HIDP_DATA_RTYPE_FEATURE 0x03
 
 #define SIXAXIS 1
@@ -66,342 +66,371 @@
 
 typedef struct motion_dev {
-    int index;
-    bdaddr_t addr;
-    char type;
-    int csk;
-    int isk;
-    struct motion_dev *next;
+  int index;
+  bdaddr_t addr;
+  char type;
+  int csk;
+  int isk;
+  struct motion_dev *next;
 } motion_dev_t;
 
-bdaddr_t bdaddr_any={{0,0,0,0,0,0}};
-
-namespace flair
-{
-namespace sensor
-{
-
-DualShock3::DualShock3(const FrameworkManager* parent,string name,string receiverAddress,int receiverPort,ConnectionType_t _connectionType,uint32_t period,uint32_t bitsPerAxis,uint8_t priority)
-    :HostEthController(parent,name,receiverAddress,receiverPort,period,bitsPerAxis,priority),connectionType(_connectionType) {
-    controllerName="DualShock3";
-    last_voltage_time=0;
-
-    // axis stuff
-    axisNumber=4;
-    nativeBitsPerAxis=8;
-    cvmatrix_descriptor *axisDescriptor=new cvmatrix_descriptor(axisNumber,1);
-    for (unsigned int i=0;i<axisNumber;i++) {
-        axisDescriptor->SetElementName(i,0,GetAxisDescription(i));
-    }
-    axis=new cvmatrix((IODevice*)this,axisDescriptor,Int16Type);
-    AddDataToLog(axis);
-
-    // buttons stuff
-    buttonNumber=16;
-    cvmatrix_descriptor *buttonDescriptor=new cvmatrix_descriptor(buttonNumber,1);
-    for (unsigned int i=0;i<buttonNumber;i++) {
-        buttonDescriptor->SetElementName(i,0,GetButtonDescription(i));
-    }
-    button=new cvmatrix((IODevice*)this,buttonDescriptor,Int8Type);
-    AddDataToLog(button);
-
-    Tab *settingsTab=new Tab(tabWidget,"Settings");
-    dataSize=6;
-    datas=new int8_t[dataSize];
-
-    GroupBox* settingsGroupBox=new GroupBox(settingsTab->NewRow(),controllerName);
-    deadZone=new SpinBox(settingsGroupBox->NewRow(),"dead zone:",-130,130,1);
-    batteryChargeLevel=new Label(settingsGroupBox->LastRowLastCol(),"battery charge level");
-    enabled=new CheckBox(settingsGroupBox->LastRowLastCol(),"enabled");
-
-    if(connectionType==Bluetooth) {
-        //init DS3
-        usb_scan();
-
-        int csk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_CTRL);
-        isk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_INTR);
-
-        if ( csk>=0 && isk>=0 )
-            Printf("Waiting for Bluetooth connections.\n");
-        else
-            Thread::Err("Unable to listen on HID PSMs.\n");
-
-
-        fd_set fds;
-        FD_ZERO(&fds);
-
-        if ( csk >= 0 ) FD_SET(csk, &fds);
-        if ( select(csk+1,&fds,NULL,NULL,NULL) < 0 ) Thread::Err("select\n");
-        // Incoming connection ?
-
-        if ( csk>=0 && FD_ISSET(csk,&fds) )
-        {
-            //printf("accept\n");
-            dev = accept_device(csk, isk);
-            setup_device(dev);
+bdaddr_t bdaddr_any = {{0, 0, 0, 0, 0, 0}};
+
+namespace flair {
+namespace sensor {
+
+DualShock3::DualShock3(const FrameworkManager *parent, string name,
+                       string receiverAddress, int receiverPort,
+                       ConnectionType_t _connectionType, uint32_t period,
+                       uint32_t bitsPerAxis, uint8_t priority)
+    : HostEthController(parent, name, receiverAddress, receiverPort, period,
+                        bitsPerAxis, priority),
+      connectionType(_connectionType) {
+  controllerName = "DualShock3";
+  last_voltage_time = 0;
+
+  // axis stuff
+  axisNumber = 4;
+  nativeBitsPerAxis = 8;
+  cvmatrix_descriptor *axisDescriptor = new cvmatrix_descriptor(axisNumber, 1);
+  for (unsigned int i = 0; i < axisNumber; i++) {
+    axisDescriptor->SetElementName(i, 0, GetAxisDescription(i));
+  }
+  axis = new cvmatrix((IODevice *)this, axisDescriptor, Int16Type);
+  AddDataToLog(axis);
+
+  // buttons stuff
+  buttonNumber = 16;
+  cvmatrix_descriptor *buttonDescriptor =
+      new cvmatrix_descriptor(buttonNumber, 1);
+  for (unsigned int i = 0; i < buttonNumber; i++) {
+    buttonDescriptor->SetElementName(i, 0, GetButtonDescription(i));
+  }
+  button = new cvmatrix((IODevice *)this, buttonDescriptor, Int8Type);
+  AddDataToLog(button);
+
+  Tab *settingsTab = new Tab(tabWidget, "Settings");
+  dataSize = 6;
+  datas = new int8_t[dataSize];
+
+  GroupBox *settingsGroupBox =
+      new GroupBox(settingsTab->NewRow(), controllerName);
+  deadZone =
+      new SpinBox(settingsGroupBox->NewRow(), "dead zone:", -130, 130, 1);
+  batteryChargeLevel =
+      new Label(settingsGroupBox->LastRowLastCol(), "battery charge level");
+  enabled = new CheckBox(settingsGroupBox->LastRowLastCol(), "enabled");
+
+  if (connectionType == Bluetooth) {
+    // init DS3
+    usb_scan();
+
+    int csk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_CTRL);
+    isk = l2cap_listen(&bdaddr_any, L2CAP_PSM_HIDP_INTR);
+
+    if (csk >= 0 && isk >= 0)
+      Printf("Waiting for Bluetooth connections.\n");
+    else
+      Thread::Err("Unable to listen on HID PSMs.\n");
+
+    fd_set fds;
+    FD_ZERO(&fds);
+
+    if (csk >= 0)
+      FD_SET(csk, &fds);
+    if (select(csk + 1, &fds, NULL, NULL, NULL) < 0)
+      Thread::Err("select\n");
+    // Incoming connection ?
+
+    if (csk >= 0 && FD_ISSET(csk, &fds)) {
+      // printf("accept\n");
+      dev = accept_device(csk, isk);
+      setup_device(dev);
+    }
+  } else if (connectionType == Usb) {
+    int nr, i;
+    unsigned char buf[128];
+
+    for (i = 0; i < 255; i++) {
+      ostringstream dev_name;
+      dev_name << "/dev/hidraw" << i;
+      if ((usb_fd = open(dev_name.str().c_str(), O_RDONLY)) >= 0) {
+        int res = 0;
+        struct hidraw_devinfo info;
+
+        res = ioctl(usb_fd, HIDIOCGRAWINFO, &info);
+        if (res < 0) {
+          Thread::Err("ioctl error (HIDIOCGRAWINFO) on %s\n",
+                      dev_name.str().c_str());
+        } else {
+          // Printf("%x %x\n", info.vendor, info.product);
+          if (info.vendor == 0x054c && info.product == 0x0268) {
+            Printf("successfully opened %s\n", dev_name.str().c_str());
+            Printf("Press PS button to turn the controller on\n");
+            break;
+          }
         }
-    } else if(connectionType==Usb) {
-        int nr,i;
-        unsigned char buf[128];
-
-        for(i=0; i<255; i++) {
-            ostringstream dev_name;
-            dev_name << "/dev/hidraw" << i;
-            if ((usb_fd = open(dev_name.str().c_str(), O_RDONLY)) >= 0) {
-                int res = 0;
-                struct hidraw_devinfo info;
-
-                res = ioctl(usb_fd, HIDIOCGRAWINFO, &info);
-                if (res < 0) {
-                    Thread::Err("ioctl error (HIDIOCGRAWINFO) on %s\n",dev_name.str().c_str());
-                } else {
-                    //Printf("%x %x\n", info.vendor, info.product);
-                    if(info.vendor==0x054c && info.product==0x0268) {
-                        Printf("successfully opened %s\n",dev_name.str().c_str());
-                        Printf("Press PS button to turn the controller on\n");
-                        break;
-                    }
-
-                }
-                close(usb_fd);
-            }
+        close(usb_fd);
+      }
+    }
+    if (i == 255) {
+      Thread::Err("sixad-raw::open(hidrawX) - failed to open hidraw device\n");
+      return;
+    }
+
+    // block until PS button is pressed
+    if ((nr = read(usb_fd, buf, sizeof(buf))) < 0) {
+      Thread::Err("sixad-raw::read(fd) - failed to read from device\n");
+    }
+
+    if (nr < 49 || nr > 50) {
+      Thread::Err("sixad-raw::read(fd) - not a sixaxis (nr = %i )\n", nr);
+    }
+  }
+  ledmask = 0;
+}
+
+DualShock3::~DualShock3() {
+  if (connectionType == Usb) {
+    close(usb_fd);
+  }
+  if (connectionType == Bluetooth) {
+    if (!popen("/etc/init.d/bluetooth restart", "r"))
+      Thread::Warn("Could not restart bluetooth service\n");
+  }
+}
+
+string DualShock3::GetAxisDescription(unsigned int axis) {
+  string description;
+
+  switch (axis) {
+  case 0:
+    description = "left stick x-axis";
+    break;
+  case 1:
+    description = "left stick y-axis";
+    break;
+  case 2:
+    description = "right stick x-axis";
+    break;
+  case 3:
+    description = "right stick y-axis";
+    break;
+  }
+  return description;
+}
+
+string DualShock3::GetButtonDescription(unsigned int button) {
+  switch (button) {
+  case 0:
+    return "start";
+    break;
+  case 1:
+    return "select";
+    break;
+  case 2:
+    return "square";
+    break;
+  case 3:
+    return "triangle";
+    break;
+  case 4:
+    return "circle";
+    break;
+  case 5:
+    return "cross";
+    break;
+  case 6:
+    return "left 1";
+    break;
+  case 7:
+    return "left 2";
+    break;
+  case 8:
+    return "left 3";
+    break;
+  case 9:
+    return "right 1";
+    break;
+  case 10:
+    return "right 2";
+    break;
+  case 11:
+    return "right 3";
+    break;
+  case 12:
+    return "up";
+    break;
+  case 13:
+    return "down";
+    break;
+  case 14:
+    return "left";
+    break;
+  case 15:
+    return "right";
+    break;
+  }
+}
+
+bool DualShock3::IsDataFrameReady() {
+  unsigned char report[256];
+  unsigned char tmp_report[256];
+
+  if (!enabled->IsChecked()) {
+    meaningfulDataAvailable = false;
+    usleep(100000);
+    return false;
+  }
+  now = GetTime();
+  if (connectionType == Bluetooth) {
+    fd_set fds;
+    FD_ZERO(&fds);
+    int fdmax = 0; /*
+     if ( isk >= 0 ) FD_SET(isk, &fds);
+     if ( isk > fdmax ) fdmax = isk;
+*/
+    FD_SET(dev->isk, &fds);
+    if (dev->isk > fdmax)
+      fdmax = dev->isk;
+
+    if (select(fdmax + 1, &fds, NULL, NULL, NULL) < 0)
+      fatal("select");
+
+    // Incoming input report ?
+    if (FD_ISSET(dev->isk, &fds)) {
+      int nr;
+      int recv_result;
+      bool flushed = false;
+      while (!flushed) {
+        recv_result = recv(dev->isk, tmp_report, sizeof(report), MSG_DONTWAIT);
+        if (recv_result <= 0) {
+          if ((errno != EAGAIN) && (errno != EWOULDBLOCK)) {
+            fprintf(stderr, "%d disconnected\n", dev->index);
+            close(dev->csk);
+            close(dev->isk);
+            free(dev);
+            return false;
+          } else {
+            flushed = true;
+            // fprintf(stderr, "\n");
+            continue;
+          }
+        } else {
+          // fprintf(stderr, ".");
+          nr = recv_result;
+          memcpy(report, tmp_report, nr);
         }
-        if(i==255) {
-            Thread::Err("sixad-raw::open(hidrawX) - failed to open hidraw device\n");
-            return;
-        }
-
-        // block until PS button is pressed
-        if ((nr=read(usb_fd, buf, sizeof(buf))) < 0) {
-            Thread::Err("sixad-raw::read(fd) - failed to read from device\n");
-        }
-
-        if (nr < 49 || nr > 50) {
-            Thread::Err("sixad-raw::read(fd) - not a sixaxis (nr = %i )\n",nr);
-        }
-    }
-    ledmask=0;
-}
-
-DualShock3::~DualShock3() {
-    if(connectionType==Usb) {
-        close(usb_fd);
-    }
-    if(connectionType==Bluetooth) {
-        if (!popen("/etc/init.d/bluetooth restart","r"))
-            Thread::Warn("Could not restart bluetooth service\n");
-    }
-}
-
-string DualShock3::GetAxisDescription(unsigned int axis) {
-    string description;
-
-    switch(axis) {
-    case 0:
-        description="left stick x-axis";
-        break;
-    case 1:
-        description="left stick y-axis";
-        break;
-    case 2:
-        description="right stick x-axis";
-        break;
-    case 3:
-        description="right stick y-axis";
-        break;
-    }
-    return description;
-}
-
-string DualShock3::GetButtonDescription(unsigned int button) {
-    switch(button) {
-    case 0:
-        return "start";break;
-    case 1:
-        return "select";break;
-    case 2:
-        return "square";break;
-    case 3:
-        return "triangle";break;
-    case 4:
-        return "circle";break;
-    case 5:
-        return "cross";break;
-    case 6:
-        return "left 1";break;
-    case 7:
-        return "left 2";break;
-    case 8:
-        return "left 3";break;
-    case 9:
-        return "right 1";break;
-    case 10:
-        return "right 2";break;
-    case 11:
-        return "right 3";break;
-    case 12:
-        return "up";break;
-    case 13:
-        return "down";break;
-    case 14:
-        return "left";break;
-    case 15:
-        return "right";break;
-    }
-}
-
-bool DualShock3::IsDataFrameReady() {
-    unsigned char report[256];
-    unsigned char tmp_report[256];
-
-    if(!enabled->IsChecked()) {
-        meaningfulDataAvailable=false;
-        usleep(100000);
-        return false;
-    }
-    now=GetTime();
-    if(connectionType==Bluetooth) {
-        fd_set fds;
-        FD_ZERO(&fds);
-        int fdmax = 0;/*
-        if ( isk >= 0 ) FD_SET(isk, &fds);
-        if ( isk > fdmax ) fdmax = isk;
-*/
-        FD_SET(dev->isk, &fds);
-        if ( dev->isk > fdmax ) fdmax = dev->isk;
-
-        if ( select(fdmax+1,&fds,NULL,NULL,NULL) < 0 ) fatal("select");
-
-        // Incoming input report ?
-        if ( FD_ISSET(dev->isk, &fds) ) {
-            int nr;
-            int recv_result;
-            bool flushed=false;
-            while(!flushed) {
-                recv_result=recv(dev->isk, tmp_report, sizeof(report), MSG_DONTWAIT);
-                if (recv_result<=0) {
-                    if ((errno!=EAGAIN)&&(errno!=EWOULDBLOCK)) {
-                        fprintf(stderr, "%d disconnected\n", dev->index);
-                        close(dev->csk);
-                        close(dev->isk);
-                        free(dev);
-                        return false;
-                    } else {
-                        flushed=true;
-                        //fprintf(stderr, "\n");
-                        continue;
-                    }
-                } else {
-                    //fprintf(stderr, ".");
-                    nr=recv_result;
-                    memcpy(report,tmp_report,nr);
-                }
-            }
-            if (report[0] == 0xa1) {
-              return parse_report_sixaxis_ds3(report+1, nr-1);
-            }
-        }
-        return false;
-
-    } else if(connectionType==Usb) {
-        int nr =read(usb_fd, report, sizeof(report));
-        return parse_report_sixaxis_ds3(report, nr);
+      }
+      if (report[0] == 0xa1) {
+        return parse_report_sixaxis_ds3(report + 1, nr - 1);
+      }
     }
     return false;
+
+  } else if (connectionType == Usb) {
+    int nr = read(usb_fd, report, sizeof(report));
+    return parse_report_sixaxis_ds3(report, nr);
+  }
+  return false;
 }
 
 bool DualShock3::parse_report_sixaxis_ds3(unsigned char *r, int len) {
-    if ( r[0]==0x01 && r[1]==0 &&len>=49 ) {
-        datas[0]=r[2];
-        datas[1]=r[3];
-        datas[2]=compute_dead_zone(0,r[6]);
-        datas[3]=compute_dead_zone(1,r[7]);
-        datas[4]=compute_dead_zone(2,r[8]);
-        datas[5]=compute_dead_zone(3,r[9]);
-
-        if(GetTime()>(last_voltage_time+5*(Time)1000000000)) {
-            //toute les 5 secondes
-            //report battery charge level
-            if(connectionType==Bluetooth) {
-                batteryChargeLevel->SetText("battery: %i/5",r[30]);
-            }
-            if(connectionType==Usb) {
-                batteryChargeLevel->SetText("battery: usb connected");
-            }
-            last_voltage_time=GetTime();
-        }
-
-        return true;
-    }
-    return false;
+  if (r[0] == 0x01 && r[1] == 0 && len >= 49) {
+    datas[0] = r[2];
+    datas[1] = r[3];
+    datas[2] = compute_dead_zone(0, r[6]);
+    datas[3] = compute_dead_zone(1, r[7]);
+    datas[4] = compute_dead_zone(2, r[8]);
+    datas[5] = compute_dead_zone(3, r[9]);
+
+    if (GetTime() > (last_voltage_time + 5 * (Time)1000000000)) {
+      // toute les 5 secondes
+      // report battery charge level
+      if (connectionType == Bluetooth) {
+        batteryChargeLevel->SetText("battery: %i/5", r[30]);
+      }
+      if (connectionType == Usb) {
+        batteryChargeLevel->SetText("battery: usb connected");
+      }
+      last_voltage_time = GetTime();
+    }
+
+    return true;
+  }
+  return false;
 }
 
 void DualShock3::GetAxisData() {
 
-    axis->GetMutex();
-//    axis->SetValueNoMutex(0, 0,datas[2]/(float)X_AXIS_RANGE); //left stick x-axis
-//    axis->SetValueNoMutex(1, 0,datas[3]/(float)Y_AXIS_RANGE); //left stick y-axis
-//    axis->SetValueNoMutex(2, 0,datas[4]/(float)X_AXIS_RANGE); //right stick x-axis
-//    axis->SetValueNoMutex(3, 0,datas[5]/(float)Y_AXIS_RANGE); //right stick y-axis
-    axis->SetValueNoMutex(0, 0,datas[2]); //left stick x-axis
-    axis->SetValueNoMutex(1, 0,datas[3]); //left stick y-axis
-    axis->SetValueNoMutex(2, 0,datas[4]); //right stick x-axis
-    axis->SetValueNoMutex(3, 0,datas[5]); //right stick y-axis
-    axis->ReleaseMutex();
-    axis->SetDataTime(now);
+  axis->GetMutex();
+  //    axis->SetValueNoMutex(0, 0,datas[2]/(float)X_AXIS_RANGE); //left stick
+  //    x-axis
+  //    axis->SetValueNoMutex(1, 0,datas[3]/(float)Y_AXIS_RANGE); //left stick
+  //    y-axis
+  //    axis->SetValueNoMutex(2, 0,datas[4]/(float)X_AXIS_RANGE); //right stick
+  //    x-axis
+  //    axis->SetValueNoMutex(3, 0,datas[5]/(float)Y_AXIS_RANGE); //right stick
+  //    y-axis
+  axis->SetValueNoMutex(0, 0, datas[2]); // left stick x-axis
+  axis->SetValueNoMutex(1, 0, datas[3]); // left stick y-axis
+  axis->SetValueNoMutex(2, 0, datas[4]); // right stick x-axis
+  axis->SetValueNoMutex(3, 0, datas[5]); // right stick y-axis
+  axis->ReleaseMutex();
+  axis->SetDataTime(now);
 }
 
 void DualShock3::GetButtonData() {
-    //static uint8_t old_start_button=0;
-    button->GetMutex();
-    button->SetValueNoMutex(0, 0,(datas[0]&0x08)==0?0:1); //start
-/*
-    uint8_t start_button=datas[0]&0x08;
-    if (start_button!=old_start_button) {
-        if (start_button==0) {
-            Thread::Info("Debug: start button released\n");
-        } else {
-            Thread::Info("Debug: start button pressed\n");
-        }
-        old_start_button=start_button;
-    }
-*/
-    button->SetValueNoMutex(1, 0,(datas[0]&0x01)==0?0:1);  //select
-    button->SetValueNoMutex(2, 0,(datas[1]&0x80)==0?0:1);  //square
-    button->SetValueNoMutex(3, 0,(datas[1]&0x10)==0?0:1);  //triangle
-    button->SetValueNoMutex(4, 0,(datas[1]&0x20)==0?0:1);  //circle
-    button->SetValueNoMutex(5, 0,(datas[1]&0x40)==0?0:1);  //cross
-    button->SetValueNoMutex(6, 0,(datas[1]&0x04)==0?0:1);  //left 1
-    button->SetValueNoMutex(7, 0,(datas[1]&0x01)==0?0:1);  //left 2
-    button->SetValueNoMutex(8, 0,(datas[0]&0x02)==0?0:1);  //left 3
-    button->SetValueNoMutex(9, 0,(datas[1]&0x08)==0?0:1);  //right 1
-    button->SetValueNoMutex(10, 0,(datas[1]&0x02)==0?0:1); //right 2
-    button->SetValueNoMutex(11, 0,(datas[0]&0x04)==0?0:1); //right 3
-    button->SetValueNoMutex(12, 0,(datas[0]&0x10)==0?0:1); //up
-    button->SetValueNoMutex(13, 0,(datas[0]&0x40)==0?0:1); //down
-    button->SetValueNoMutex(14, 0,(datas[0]&0x80)==0?0:1); //left
-    button->SetValueNoMutex(15, 0,(datas[0]&0x20)==0?0:1); //right
-    button->ReleaseMutex();
-    button->SetDataTime(now);
+  // static uint8_t old_start_button=0;
+  button->GetMutex();
+  button->SetValueNoMutex(0, 0, (datas[0] & 0x08) == 0 ? 0 : 1); // start
+  /*
+      uint8_t start_button=datas[0]&0x08;
+      if (start_button!=old_start_button) {
+          if (start_button==0) {
+              Thread::Info("Debug: start button released\n");
+          } else {
+              Thread::Info("Debug: start button pressed\n");
+          }
+          old_start_button=start_button;
+      }
+  */
+  button->SetValueNoMutex(1, 0, (datas[0] & 0x01) == 0 ? 0 : 1); // select
+  button->SetValueNoMutex(2, 0, (datas[1] & 0x80) == 0 ? 0 : 1); // square
+  button->SetValueNoMutex(3, 0, (datas[1] & 0x10) == 0 ? 0 : 1); // triangle
+  button->SetValueNoMutex(4, 0, (datas[1] & 0x20) == 0 ? 0 : 1); // circle
+  button->SetValueNoMutex(5, 0, (datas[1] & 0x40) == 0 ? 0 : 1); // cross
+  button->SetValueNoMutex(6, 0, (datas[1] & 0x04) == 0 ? 0 : 1); // left 1
+  button->SetValueNoMutex(7, 0, (datas[1] & 0x01) == 0 ? 0 : 1); // left 2
+  button->SetValueNoMutex(8, 0, (datas[0] & 0x02) == 0 ? 0 : 1); // left 3
+  button->SetValueNoMutex(9, 0, (datas[1] & 0x08) == 0 ? 0 : 1); // right 1
+  button->SetValueNoMutex(10, 0, (datas[1] & 0x02) == 0 ? 0 : 1); // right 2
+  button->SetValueNoMutex(11, 0, (datas[0] & 0x04) == 0 ? 0 : 1); // right 3
+  button->SetValueNoMutex(12, 0, (datas[0] & 0x10) == 0 ? 0 : 1); // up
+  button->SetValueNoMutex(13, 0, (datas[0] & 0x40) == 0 ? 0 : 1); // down
+  button->SetValueNoMutex(14, 0, (datas[0] & 0x80) == 0 ? 0 : 1); // left
+  button->SetValueNoMutex(15, 0, (datas[0] & 0x20) == 0 ? 0 : 1); // right
+  button->ReleaseMutex();
+  button->SetDataTime(now);
 }
 
 void DualShock3::ProcessMessage(core::Message *controllerAction) {
-    ControllerAction action;
-    memcpy(&action,controllerAction->buffer,sizeof(ControllerAction));
-    if (action==ControllerAction::SetLedOn) {
-        Thread::Info("LedOn action request\n");
-    } else if (action==ControllerAction::SetLedOff) {
-        Thread::Info("LedOff action request\n");
-    } else if (action==ControllerAction::Rumble) {
-        Thread::Info("Rumble action request\n");
-    } else if (action==ControllerAction::FlashLed) {
-        Thread::Info("FlashLed action request\n");
-    }
-// (char *msg, int msgSize)
-/*    for (unsigned int i=0; i<4; i++) {
-        if(msg[4+2*i]!=0 || msg[5+2*i]!=0) set_led(i+1,msg[4+2*i],msg[5+2*i]);
-
-    }
-    if(msg[0]!=0 || msg[2]!=0) rumble(msg[0],msg[1],msg[2],msg[3]);
-        */
+  ControllerAction action;
+  memcpy(&action, controllerAction->buffer, sizeof(ControllerAction));
+  if (action == ControllerAction::SetLedOn) {
+    Thread::Info("LedOn action request\n");
+  } else if (action == ControllerAction::SetLedOff) {
+    Thread::Info("LedOff action request\n");
+  } else if (action == ControllerAction::Rumble) {
+    Thread::Info("Rumble action request\n");
+  } else if (action == ControllerAction::FlashLed) {
+    Thread::Info("FlashLed action request\n");
+  }
+  // (char *msg, int msgSize)
+  /*    for (unsigned int i=0; i<4; i++) {
+          if(msg[4+2*i]!=0 || msg[5+2*i]!=0) set_led(i+1,msg[4+2*i],msg[5+2*i]);
+
+      }
+      if(msg[0]!=0 || msg[2]!=0) rumble(msg[0],msg[1],msg[2],msg[3]);
+          */
 }
 
@@ -410,26 +439,30 @@
 
 int DualShock3::mystr2ba(const char *s, bdaddr_t *ba) {
-    if ( strlen(s) != 17 ) return 1;
-    for ( int i=0; i<6; ++i ) {
-        int d = strtol(s+15-3*i, NULL, 16);
-        if ( d<0 || d>255 ) return 1;
-        ba->b[i] = d;
-    }
-    return 0;
+  if (strlen(s) != 17)
+    return 1;
+  for (int i = 0; i < 6; ++i) {
+    int d = strtol(s + 15 - 3 * i, NULL, 16);
+    if (d < 0 || d > 255)
+      return 1;
+    ba->b[i] = d;
+  }
+  return 0;
 }
 
 char *DualShock3::myba2str(const bdaddr_t *ba) {
-    static char buf[2][18];  // Static buffer valid for two invocations.
-    static int index = 0;
-    index = (index+1)%2;
-    sprintf(buf[index], "%02x:%02x:%02x:%02x:%02x:%02x",
-            ba->b[5], ba->b[4], ba->b[3], ba->b[2], ba->b[1], ba->b[0]);
-    return buf[index];
+  static char buf[2][18]; // Static buffer valid for two invocations.
+  static int index = 0;
+  index = (index + 1) % 2;
+  sprintf(buf[index], "%02x:%02x:%02x:%02x:%02x:%02x", ba->b[5], ba->b[4],
+          ba->b[3], ba->b[2], ba->b[1], ba->b[0]);
+  return buf[index];
 }
 
 void DualShock3::fatal(const char *msg) {
-    if ( errno ) perror(msg);
-    else fprintf(stderr, "%s\n", msg);
-    exit(1);
+  if (errno)
+    perror(msg);
+  else
+    fprintf(stderr, "%s\n", msg);
+  exit(1);
 }
 
@@ -439,49 +472,53 @@
 
 int DualShock3::l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm) {
-    int sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
-    if ( sk < 0 ) fatal("socket");
-
-    struct sockaddr_l2 addr;
-    addr.l2_family = AF_BLUETOOTH;
-    addr.l2_bdaddr = *BDADDR_ANY;
-    addr.l2_psm = htobs(psm);
-    addr.l2_cid = 0;
-
-    if ( bind(sk, (struct sockaddr *)&addr, sizeof(addr)) < 0 )
-    {
-        close(sk);
-        fatal("bind");
-    }
-
-    if ( listen(sk, 5) < 0 ) fatal("listen");
-    return sk;
+  int sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+  if (sk < 0)
+    fatal("socket");
+
+  struct sockaddr_l2 addr;
+  addr.l2_family = AF_BLUETOOTH;
+  addr.l2_bdaddr = *BDADDR_ANY;
+  addr.l2_psm = htobs(psm);
+  addr.l2_cid = 0;
+
+  if (bind(sk, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+    close(sk);
+    fatal("bind");
+  }
+
+  if (listen(sk, 5) < 0)
+    fatal("listen");
+  return sk;
 }
 
 struct motion_dev *DualShock3::accept_device(int csk, int isk) {
-    Printf("Incoming connection...\n");
-    struct motion_dev *dev = (motion_dev*)malloc(sizeof(struct motion_dev));
-    if (!dev) fatal("malloc");
-
-    dev->csk = accept(csk, NULL, NULL);
-    if ( dev->csk < 0 ) fatal("accept(CTRL)");
-    dev->isk = accept(isk, NULL, NULL);
-    if ( dev->isk < 0 ) fatal("accept(INTR)");
-
-    struct sockaddr_l2 addr;
-    socklen_t addrlen = sizeof(addr);
-    if ( getpeername(dev->isk, (struct sockaddr *)&addr, &addrlen) < 0 ) fatal("getpeername");
-    dev->addr = addr.l2_bdaddr;
-
-    // Distinguish SIXAXIS / DS3
-    unsigned char resp[64];
-    char get03f2[] = { HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_FEATURE | 8,
-                       (char)0xf2, sizeof(resp), sizeof(resp)>>8
-                     };
-    send(dev->csk, get03f2, sizeof(get03f2), 0);  // 0301 is interesting too.
-    int nr = recv(dev->csk, resp, sizeof(resp), 0);
-
-    dev->type = (resp[13]==0x40) ? SIXAXIS : DUALSHOCK3;  // My guess.
-
-    return dev;
+  Printf("Incoming connection...\n");
+  struct motion_dev *dev = (motion_dev *)malloc(sizeof(struct motion_dev));
+  if (!dev)
+    fatal("malloc");
+
+  dev->csk = accept(csk, NULL, NULL);
+  if (dev->csk < 0)
+    fatal("accept(CTRL)");
+  dev->isk = accept(isk, NULL, NULL);
+  if (dev->isk < 0)
+    fatal("accept(INTR)");
+
+  struct sockaddr_l2 addr;
+  socklen_t addrlen = sizeof(addr);
+  if (getpeername(dev->isk, (struct sockaddr *)&addr, &addrlen) < 0)
+    fatal("getpeername");
+  dev->addr = addr.l2_bdaddr;
+
+  // Distinguish SIXAXIS / DS3
+  unsigned char resp[64];
+  char get03f2[] = {HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_FEATURE | 8,
+                    (char)0xf2, sizeof(resp), sizeof(resp) >> 8};
+  send(dev->csk, get03f2, sizeof(get03f2), 0); // 0301 is interesting too.
+  int nr = recv(dev->csk, resp, sizeof(resp), 0);
+
+  dev->type = (resp[13] == 0x40) ? SIXAXIS : DUALSHOCK3; // My guess.
+
+  return dev;
 }
 
@@ -495,23 +532,24 @@
 
 void DualShock3::hidp_trans(int csk, char *buf, int len) {
-    if ( send(csk, buf, len, 0) != len ) fatal("send(CTRL)");
-    char ack;
-    int nr = recv(csk, &ack, sizeof(ack), 0);
-    if ( nr!=1 || ack!=0 ) fatal("ack");
+  if (send(csk, buf, len, 0) != len)
+    fatal("send(CTRL)");
+  char ack;
+  int nr = recv(csk, &ack, sizeof(ack), 0);
+  if (nr != 1 || ack != 0)
+    fatal("ack");
 }
 
 void DualShock3::setup_device(struct motion_dev *dev) {
 
-    switch ( dev->type ) {
-    case SIXAXIS:
-    case DUALSHOCK3:
-        // Enable reporting
-        char set03f4[] = { HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE, (char)0xf4,
-                           0x42, 0x03, 0x00, 0x00
-                        };
-        hidp_trans(dev->csk, set03f4, sizeof(set03f4));
-
-        break;
-    }
+  switch (dev->type) {
+  case SIXAXIS:
+  case DUALSHOCK3:
+    // Enable reporting
+    char set03f4[] = {HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE,
+                      (char)0xf4, 0x42, 0x03, 0x00, 0x00};
+    hidp_trans(dev->csk, set03f4, sizeof(set03f4));
+
+    break;
+  }
 }
 
@@ -519,24 +557,29 @@
 // Reports
 
-//faire un reglage station sol pour dead zone
-int8_t DualShock3::compute_dead_zone(int axis,unsigned char value)
-{
-    float tmp;
-
-    if(value>128+deadZone->Value()) {
-        tmp=(value-deadZone->Value()-128.)*128./(128.-deadZone->Value());
-    } else if(value<128-deadZone->Value()) {
-        //return value+DEAD_ZONE-128;
-        tmp=(value+deadZone->Value()-128.)*127./(128.-deadZone->Value());
-    } else {
-        return 0;
-    }
-
-    if(tmp>127) return 127;
-    if(tmp<-127) return -127;
-    if(tmp>((int8_t)tmp+.5) && tmp>0) return (int8_t)(tmp+1);
-    if(tmp<((int8_t)tmp-.5) && tmp<0) return (int8_t)(tmp-1);
-
-    return (int8_t)tmp;
+// faire un reglage station sol pour dead zone
+int8_t DualShock3::compute_dead_zone(int axis, unsigned char value) {
+  float tmp;
+
+  if (value > 128 + deadZone->Value()) {
+    tmp =
+        (value - deadZone->Value() - 128.) * 128. / (128. - deadZone->Value());
+  } else if (value < 128 - deadZone->Value()) {
+    // return value+DEAD_ZONE-128;
+    tmp =
+        (value + deadZone->Value() - 128.) * 127. / (128. - deadZone->Value());
+  } else {
+    return 0;
+  }
+
+  if (tmp > 127)
+    return 127;
+  if (tmp < -127)
+    return -127;
+  if (tmp > ((int8_t)tmp + .5) && tmp > 0)
+    return (int8_t)(tmp + 1);
+  if (tmp < ((int8_t)tmp - .5) && tmp < 0)
+    return (int8_t)(tmp - 1);
+
+  return (int8_t)tmp;
 }
 
@@ -544,195 +587,179 @@
 // USB functions
 
-void DualShock3::usb_pair_device(struct usb_device *dev, int itfnum)
-{
-
-    usb_dev_handle *devh = usb_open(dev);
-    if ( ! devh ) fatal("usb_open");
-    usb_detach_kernel_driver_np(devh, itfnum);
-    int res = usb_claim_interface(devh, itfnum);
-    if ( res < 0 ) fatal("usb_claim_interface");
-
-    bdaddr_t current_ba;  // Current pairing address.
-
-    switch ( dev->descriptor.idProduct ) {
+void DualShock3::usb_pair_device(struct usb_device *dev, int itfnum) {
+
+  usb_dev_handle *devh = usb_open(dev);
+  if (!devh)
+    fatal("usb_open");
+  usb_detach_kernel_driver_np(devh, itfnum);
+  int res = usb_claim_interface(devh, itfnum);
+  if (res < 0)
+    fatal("usb_claim_interface");
+
+  bdaddr_t current_ba; // Current pairing address.
+
+  switch (dev->descriptor.idProduct) {
+  case PRODUCT_SIXAXIS_DS3: {
+    // remote_printf("USB: SIXAXIS/DS3\n");
+    char msg[8];
+    res =
+        usb_control_msg(devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+                        USB_GET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
+    /*
+            unsigned char msg[8];
+            res = usb_control_msg
+                  (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+                   USB_GET_REPORT, 0x03f5, itfnum, (void*)msg, sizeof(msg),
+       5000);*/
+    if (res < 0)
+      fatal("usb_control_msg(read master)");
+    for (int i = 0; i < 6; ++i)
+      current_ba.b[i] = (uint8_t)msg[7 - i];
+    break;
+  }
+  }
+
+  // New pairing address
+  int dev_id;
+  dev_id = hci_get_route(NULL);
+  struct hci_dev_info di;
+  hci_devinfo(dev_id, &di);
+
+  // Perform pairing.
+  if (!bacmp(&current_ba, &di.bdaddr)) {
+    printf("  Already paired to %s\n", myba2str(&di.bdaddr));
+  } else {
+    printf("  Changing master from %s to %s\n", myba2str(&current_ba),
+           myba2str(&di.bdaddr));
+    switch (dev->descriptor.idProduct) {
     case PRODUCT_SIXAXIS_DS3: {
-        //remote_printf("USB: SIXAXIS/DS3\n");
-        char msg[8];
-        res = usb_control_msg
-              (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
-               USB_GET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
-/*
-        unsigned char msg[8];
-        res = usb_control_msg
-              (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
-               USB_GET_REPORT, 0x03f5, itfnum, (void*)msg, sizeof(msg), 5000);*/
-        if ( res < 0 ) fatal("usb_control_msg(read master)");
-        for ( int i=0; i<6; ++i ) current_ba.b[i] = (uint8_t)msg[7-i];
-        break;
-    }
-    }
-
-    // New pairing address
-    int dev_id;
-    dev_id = hci_get_route(NULL);
-    struct hci_dev_info di;
-    hci_devinfo(dev_id, &di);
-
-
-    // Perform pairing.
-    if ( ! bacmp(&current_ba, &di.bdaddr) )
-    {
-        printf("  Already paired to %s\n", myba2str(&di.bdaddr));
-    }
-    else
-    {
-        printf("  Changing master from %s to %s\n",
-               myba2str(&current_ba), myba2str(&di.bdaddr));
-        switch ( dev->descriptor.idProduct )
-        {
-        case PRODUCT_SIXAXIS_DS3:
-        {
-            char msg[8] =
-            { 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] };
-            res = usb_control_msg
-                  (devh, USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
-                   USB_SET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
-            if ( res < 0 ) fatal("usb_control_msg(write master)");
-            break;
+      char msg[8] = {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]};
+      res = usb_control_msg(
+          devh, USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+          USB_SET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
+      if (res < 0)
+        fatal("usb_control_msg(write master)");
+      break;
+    }
+    }
+  }
+
+  if (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3)
+    printf("  Now unplug the USB cable and press the PS button.\n");
+  else
+    printf("  Now press the PS button.\n");
+}
+
+void DualShock3::usb_scan() {
+  usb_init();
+  if (usb_find_busses() < 0)
+    fatal("usb_find_busses");
+  if (usb_find_devices() < 0)
+    fatal("usb_find_devices");
+  struct usb_bus *busses = usb_get_busses();
+  if (!busses)
+    fatal("usb_get_busses");
+
+  struct usb_bus *bus;
+  for (bus = busses; bus; bus = bus->next) {
+    struct usb_device *dev;
+    for (dev = bus->devices; dev; dev = dev->next) {
+      struct usb_config_descriptor *cfg;
+      for (cfg = dev->config;
+           cfg < dev->config + dev->descriptor.bNumConfigurations; ++cfg) {
+        int itfnum;
+        for (itfnum = 0; itfnum < cfg->bNumInterfaces; ++itfnum) {
+          struct usb_interface *itf = &cfg->interface[itfnum];
+          struct usb_interface_descriptor *alt;
+          for (alt = itf->altsetting;
+               alt < itf->altsetting + itf->num_altsetting; ++alt) {
+            if (dev->descriptor.idVendor == VENDOR_SONY &&
+                (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3) &&
+                alt->bInterfaceClass == 3)
+              usb_pair_device(dev, itfnum);
+          }
         }
-        }
-    }
-
-    if ( dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3 )
-        printf("  Now unplug the USB cable and press the PS button.\n");
-    else
-        printf("  Now press the PS button.\n");
-}
-
-void DualShock3::usb_scan()
-{
-    usb_init();
-    if ( usb_find_busses() < 0 ) fatal("usb_find_busses");
-    if ( usb_find_devices() < 0 ) fatal("usb_find_devices");
-    struct usb_bus *busses = usb_get_busses();
-    if ( ! busses ) fatal("usb_get_busses");
-
-    struct usb_bus *bus;
-    for ( bus=busses; bus; bus=bus->next )
-    {
-        struct usb_device *dev;
-        for ( dev=bus->devices; dev; dev=dev->next)
-        {
-            struct usb_config_descriptor *cfg;
-            for ( cfg = dev->config;
-                    cfg < dev->config + dev->descriptor.bNumConfigurations;
-                    ++cfg )
-            {
-                int itfnum;
-                for ( itfnum=0; itfnum<cfg->bNumInterfaces; ++itfnum )
-                {
-                    struct usb_interface *itf = &cfg->interface[itfnum];
-                    struct usb_interface_descriptor *alt;
-                    for ( alt = itf->altsetting;
-                            alt < itf->altsetting + itf->num_altsetting;
-                            ++alt )
-                    {
-                        if ( dev->descriptor.idVendor == VENDOR_SONY &&
-                                (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3) &&
-                                alt->bInterfaceClass == 3 )
-                            usb_pair_device(dev, itfnum);
-                    }
-                }
-            }
-        }
-    }
-}
-
-
-void DualShock3::rumble(uint8_t left_force,uint8_t left_timeout,uint8_t right_force,uint8_t right_timeout)
-{
-    // printf("rumble\n");
-
-    unsigned char datas[] =
-    {
-        0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */ ,
-        0x01,
-        0x00, 0x00, 0x00, 0x00, 0x00, //rumble values
-        0x00, 0x00, 0x00, 0x00,(unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4
-
-        0xff, 0x27, led4_on, led4_off, 0x32,
-        0xff, 0x27, led3_on, led3_off, 0x32,
-        0xff, 0x27, led2_on, led2_off, 0x32,
-        0xff, 0x27, led1_on, led1_off, 0x32,
-        0x00, 0x00, 0x00, 0x00, 0x00,
-    };
-
-    datas[5] = left_timeout;     // left timeout
-    datas[6] = left_force;  // left force
-    datas[3] = right_timeout;     // right timeout
-    datas[4] = right_force;  // right force
-
-    if(connectionType==Bluetooth) {
-        hidp_trans(dev->csk, (char *)datas, sizeof(datas));
-    }
-
-}
-
-void DualShock3::set_led(uint8_t led,uint8_t on_timeout,uint8_t off_timeout)
-{
-    uint8_t mask;
-
-    switch(led) {
-    case 1:
-        led1_on=on_timeout;
-        led1_off=off_timeout;
-        mask=2;
-        break;
-    case 2:
-        led2_on=on_timeout;
-        led2_off=off_timeout;
-        mask=4;
-        break;
-    case 3:
-        led3_on=on_timeout;
-        led3_off=off_timeout;
-        mask=8;
-        break;
-    case 4:
-        led4_on=on_timeout;
-        led4_off=off_timeout;
-        mask=16;
-        break;
-    }
-
-    if(on_timeout!=0) {
-        ledmask|=mask;
-    } else {
-        ledmask&=~mask;
-    }
-    /*
-        printf("led %x\n",ledmask);
-        printf("1:%i %i\n",led1_on,led1_off);
-        printf("2:%i %i\n",led2_on,led2_off);
-        printf("3:%i %i\n",led3_on,led3_off);
-        printf("4:%i %i\n",led4_on,led4_off);*/
-
-
-    unsigned char datas[] = {
-        0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */ ,
-        0x01,
-        0x00, 0x00, 0x00, 0x00, 0x00, //rumble values
-        0x00, 0x00, 0x00, 0x00,(unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4
-
-        0xff, 0x27, led4_on, led4_off, 0x32,
-        0xff, 0x27, led3_on, led3_off, 0x32,
-        0xff, 0x27, led2_on, led2_off, 0x32,
-        0xff, 0x27, led1_on, led1_off, 0x32,
-        0x00, 0x00, 0x00, 0x00, 0x00,
-    };
-
-    if(connectionType==Bluetooth) {
-        hidp_trans(dev->csk, (char *)datas, sizeof(datas));
-    }
+      }
+    }
+  }
+}
+
+void DualShock3::rumble(uint8_t left_force, uint8_t left_timeout,
+                        uint8_t right_force, uint8_t right_timeout) {
+  // printf("rumble\n");
+
+  unsigned char datas[] = {
+      0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */, 0x01, 0x00, 0x00,
+      0x00, 0x00, 0x00, // rumble values
+      0x00, 0x00, 0x00, 0x00, (unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4
+
+      0xff, 0x27, led4_on, led4_off, 0x32, 0xff, 0x27, led3_on, led3_off, 0x32,
+      0xff, 0x27, led2_on, led2_off, 0x32, 0xff, 0x27, led1_on, led1_off, 0x32,
+      0x00, 0x00, 0x00, 0x00, 0x00,
+  };
+
+  datas[5] = left_timeout;  // left timeout
+  datas[6] = left_force;    // left force
+  datas[3] = right_timeout; // right timeout
+  datas[4] = right_force;   // right force
+
+  if (connectionType == Bluetooth) {
+    hidp_trans(dev->csk, (char *)datas, sizeof(datas));
+  }
+}
+
+void DualShock3::set_led(uint8_t led, uint8_t on_timeout, uint8_t off_timeout) {
+  uint8_t mask;
+
+  switch (led) {
+  case 1:
+    led1_on = on_timeout;
+    led1_off = off_timeout;
+    mask = 2;
+    break;
+  case 2:
+    led2_on = on_timeout;
+    led2_off = off_timeout;
+    mask = 4;
+    break;
+  case 3:
+    led3_on = on_timeout;
+    led3_off = off_timeout;
+    mask = 8;
+    break;
+  case 4:
+    led4_on = on_timeout;
+    led4_off = off_timeout;
+    mask = 16;
+    break;
+  }
+
+  if (on_timeout != 0) {
+    ledmask |= mask;
+  } else {
+    ledmask &= ~mask;
+  }
+  /*
+      printf("led %x\n",ledmask);
+      printf("1:%i %i\n",led1_on,led1_off);
+      printf("2:%i %i\n",led2_on,led2_off);
+      printf("3:%i %i\n",led3_on,led3_off);
+      printf("4:%i %i\n",led4_on,led4_off);*/
+
+  unsigned char datas[] = {
+      0x52 /* HIDP_TRANS_SET_REPORT|HIDP_DATA_RTYPE_OUPUT */, 0x01, 0x00, 0x00,
+      0x00, 0x00, 0x00, // rumble values
+      0x00, 0x00, 0x00, 0x00, (unsigned char)ledmask, // 0x10=LED1 .. 0x02=LED4
+
+      0xff, 0x27, led4_on, led4_off, 0x32, 0xff, 0x27, led3_on, led3_off, 0x32,
+      0xff, 0x27, led2_on, led2_off, 0x32, 0xff, 0x27, led1_on, led1_off, 0x32,
+      0x00, 0x00, 0x00, 0x00, 0x00,
+  };
+
+  if (connectionType == Bluetooth) {
+    hidp_trans(dev->csk, (char *)datas, sizeof(datas));
+  }
 }
 
Index: trunk/tools/Controller/DualShock3/src/DualShock3.h
===================================================================
--- trunk/tools/Controller/DualShock3/src/DualShock3.h	(revision 11)
+++ trunk/tools/Controller/DualShock3/src/DualShock3.h	(revision 16)
@@ -12,5 +12,6 @@
 //  version:    $Id: $
 //
-//  purpose:    Sony DualShock3 host side driver class. Talks to target side through ethernet.
+//  purpose:    Sony DualShock3 host side driver class. Talks to target side
+//  through ethernet.
 //
 //
@@ -28,72 +29,77 @@
 
 namespace flair {
-    namespace core {
-        class FrameworkManager;
-    }
-    namespace gui {
-        class SpinBox;
-        class Label;
-        class CheckBox;
-    }
+namespace core {
+class FrameworkManager;
+}
+namespace gui {
+class SpinBox;
+class Label;
+class CheckBox;
+}
 }
 
 struct motion_dev;
 
-namespace flair { namespace sensor {
-    /*! \class DualShock3
-    *
-    * \brief Sony DualShock3 host side driver class. Talks to target side through ethernet.
-    */
-    class DualShock3 : public HostEthController {
-    public:
-        typedef enum {
-            Usb,
-            Bluetooth
-        } ConnectionType_t;
+namespace flair {
+namespace sensor {
+/*! \class DualShock3
+*
+* \brief Sony DualShock3 host side driver class. Talks to target side through
+*ethernet.
+*/
+class DualShock3 : public HostEthController {
+public:
+  typedef enum { Usb, Bluetooth } ConnectionType_t;
 
-        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);
-        ~DualShock3();
-    private:
-        gui::SpinBox* deadZone;
-        gui::CheckBox* enabled;
-        gui::Label *batteryChargeLevel;
-        ConnectionType_t connectionType;
-        core::Time now;
+  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);
+  ~DualShock3();
 
-        std::string GetAxisDescription(unsigned int axis);
-        std::string GetButtonDescription(unsigned int button);
-        void GetAxisData();
-        void GetButtonData();
-        bool IsDataFrameReady();
-        void ProcessMessage(core::Message *controllerAction);
+private:
+  gui::SpinBox *deadZone;
+  gui::CheckBox *enabled;
+  gui::Label *batteryChargeLevel;
+  ConnectionType_t connectionType;
+  core::Time now;
 
-        void UpdateFrom(const core::io_data *data) {};
-        void fatal(const char *msg);
-        int l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm);
-        struct motion_dev *accept_device(int csk, int isk);
-        void hidp_trans(int csk, char *buf, int len);
-        void setup_device(struct motion_dev *dev);
-        bool parse_report_sixaxis_ds3(unsigned char *r, int len);
-        int mystr2ba(const char *s, bdaddr_t *ba);
-        char *myba2str(const bdaddr_t *ba);
-        int8_t compute_dead_zone(int axis,unsigned char value);
-        struct motion_dev *dev;
-        int usb_fd;
-        int isk;
-        core::Time last_voltage_time;
+  std::string GetAxisDescription(unsigned int axis);
+  std::string GetButtonDescription(unsigned int button);
+  void GetAxisData();
+  void GetButtonData();
+  bool IsDataFrameReady();
+  void ProcessMessage(core::Message *controllerAction);
 
-        int8_t *datas;
-        uint8_t dataSize;
+  void UpdateFrom(const core::io_data *data){};
+  void fatal(const char *msg);
+  int l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm);
+  struct motion_dev *accept_device(int csk, int isk);
+  void hidp_trans(int csk, char *buf, int len);
+  void setup_device(struct motion_dev *dev);
+  bool parse_report_sixaxis_ds3(unsigned char *r, int len);
+  int mystr2ba(const char *s, bdaddr_t *ba);
+  char *myba2str(const bdaddr_t *ba);
+  int8_t compute_dead_zone(int axis, unsigned char value);
+  struct motion_dev *dev;
+  int usb_fd;
+  int isk;
+  core::Time last_voltage_time;
 
-        void usb_scan();
-        void usb_pair_device(struct usb_device *dev, int itfnum);
+  int8_t *datas;
+  uint8_t dataSize;
 
-        void rumble(uint8_t left_force,uint8_t left_timeout,uint8_t right_force,uint8_t right_timeout);
-        void set_led(uint8_t led,uint8_t on_timeout,uint8_t off_timeout);
-        char ledmask;
-        uint8_t led1_on,led1_off,led2_on,led2_off,led3_on,led3_off,led4_on,led4_off;
+  void usb_scan();
+  void usb_pair_device(struct usb_device *dev, int itfnum);
 
-    };
-}}
+  void rumble(uint8_t left_force, uint8_t left_timeout, uint8_t right_force,
+              uint8_t right_timeout);
+  void set_led(uint8_t led, uint8_t on_timeout, uint8_t off_timeout);
+  char ledmask;
+  uint8_t led1_on, led1_off, led2_on, led2_off, led3_on, led3_off, led4_on,
+      led4_off;
+};
+}
+}
 
 #endif // DUALSHOCK3_H
Index: trunk/tools/Controller/DualShock3/src/main.cpp
===================================================================
--- trunk/tools/Controller/DualShock3/src/main.cpp	(revision 11)
+++ trunk/tools/Controller/DualShock3/src/main.cpp	(revision 16)
@@ -33,66 +33,77 @@
 string xml_file;
 
-void parseOptions(int argc, char** argv);
+void parseOptions(int argc, char **argv);
 
-int main(int argc, char* argv[]) {
-    parseOptions(argc,argv);
+int main(int argc, char *argv[]) {
+  parseOptions(argc, argv);
 
-    DualShock3 *joystick;
-    FrameworkManager *manager;
+  DualShock3 *joystick;
+  FrameworkManager *manager;
 
-    manager= new FrameworkManager("dualshock3");
-    manager->SetupConnection("127.0.0.1",port);
-    manager->SetupUserInterface(xml_file);
+  manager = new FrameworkManager("dualshock3");
+  manager->SetupConnection("127.0.0.1", port);
+  manager->SetupUserInterface(xml_file);
 
-    if(connection=="usb") {
-//        manager->Warn("!! adresse = %s !!\n", address.c_str());
-        joystick=new DualShock3(manager,"dualshock3",receiverAddress,receiverPort,DualShock3::Usb,period,6);
-    } else {
-        joystick=new DualShock3(manager,"dualshock3",receiverAddress,receiverPort,DualShock3::Bluetooth,period,6);
-    }
+  if (connection == "usb") {
+    //        manager->Warn("!! adresse = %s !!\n", address.c_str());
+    joystick = new DualShock3(manager, "dualshock3", receiverAddress,
+                              receiverPort, DualShock3::Usb, period, 6);
+  } else {
+    joystick = new DualShock3(manager, "dualshock3", receiverAddress,
+                              receiverPort, DualShock3::Bluetooth, period, 6);
+  }
 
-    joystick->DrawUserInterface();
+  joystick->DrawUserInterface();
 
-    if(!manager->ErrorOccured()) {
-        joystick->Start();
-        joystick->Join();
-    }
+  if (!manager->ErrorOccured()) {
+    joystick->Start();
+    joystick->Join();
+  }
 
-    delete manager;
+  delete manager;
 }
 
-void parseOptions(int argc, char** argv) {
-    try {
-        CmdLine cmd("Command description message", ' ', "0.1");
+void parseOptions(int argc, char **argv) {
+  try {
+    CmdLine cmd("Command description message", ' ', "0.1");
 
-        ValueArg<string> addressArg("a","address","data receiver address (ex: uav)",true,"127.0.0.1:20000","string");
-        cmd.add(addressArg);
+    ValueArg<string> addressArg("a", "address",
+                                "data receiver address (ex: uav)", true,
+                                "127.0.0.1:20000", "string");
+    cmd.add(addressArg);
 
-        ValueArg<string> connectionArg("c","connection","connection type (usb or bluetooth)",false,"bluetooth","string");
-        cmd.add(connectionArg);
+    ValueArg<string> connectionArg("c", "connection",
+                                   "connection type (usb or bluetooth)", false,
+                                   "bluetooth", "string");
+    cmd.add(connectionArg);
 
-        ValueArg<int> portArg("p","port","local port used to connect to the ground station",false,9000,"int");
-        cmd.add(portArg);
+    ValueArg<int> portArg("p", "port",
+                          "local port used to connect to the ground station",
+                          false, 9000, "int");
+    cmd.add(portArg);
 
-        ValueArg<int> periodArg("t","period","sending data period",false,10,"int");
-        cmd.add(periodArg);
+    ValueArg<int> periodArg("t", "period", "sending data period", false, 10,
+                            "int");
+    cmd.add(periodArg);
 
-        ValueArg<string> xmlArg("x","xml","xml file",true,"./settings.xml","string");
-        cmd.add( xmlArg );
+    ValueArg<string> xmlArg("x", "xml", "xml file", true, "./settings.xml",
+                            "string");
+    cmd.add(xmlArg);
 
-        cmd.parse(argc, argv);
+    cmd.parse(argc, argv);
 
-        // Get the value parsed by each arg.
-        string receiverAddressWithPort=addressArg.getValue();
-        int semiColonPosition = receiverAddressWithPort.find(":");
-        receiverAddress=receiverAddressWithPort.substr (0,semiColonPosition);
-        receiverPort=atoi(receiverAddressWithPort.substr(semiColonPosition+1).c_str());
-        connection=connectionArg.getValue();
-        port=portArg.getValue();
-        period=periodArg.getValue();
-        xml_file=xmlArg.getValue();
+    // Get the value parsed by each arg.
+    string receiverAddressWithPort = addressArg.getValue();
+    int semiColonPosition = receiverAddressWithPort.find(":");
+    receiverAddress = receiverAddressWithPort.substr(0, semiColonPosition);
+    receiverPort =
+        atoi(receiverAddressWithPort.substr(semiColonPosition + 1).c_str());
+    connection = connectionArg.getValue();
+    port = portArg.getValue();
+    period = periodArg.getValue();
+    xml_file = xmlArg.getValue();
 
-    } catch (ArgException &e) {   // catch any exceptions
-        cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
-    }
+  } catch (ArgException &e) { // catch any exceptions
+    cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
+  }
 }
