Changeset 16 in flair-src


Ignore:
Timestamp:
04/08/16 15:48:40 (8 years ago)
Author:
Bayard Gildas
Message:

Reformatting

Location:
trunk
Files:
16 edited

Legend:

Unmodified
Added
Removed

  • trunk/demos/Sinus/src/Loop.cpp

    r14 r16  
    1111/*********************************************************************/
    1212
    13 //include header of the class
     13// include header of the class
    1414#include "Loop.h"
    15 //include only header of class declared in Loop.h (forward declaration)
     15// include only header of class declared in Loop.h (forward declaration)
    1616#include "Sinus.h"
    1717#include "MeanFilter.h"
    … …  
    2626#include <cvmatrix.h>
    2727
    28 
    2928using namespace std;
    3029using namespace flair::core;
    … …  
    3332using namespace flair::sensor;
    3433
    35 Loop::Loop(FrameworkManager* parent,string name,int priority): Thread(parent,name,priority) {
    36     Tab *mainTab=new Tab(parent->GetTabWidget(),ObjectName());
    37     killButton=new PushButton(mainTab->NewRow(),"kill");
    38     startLogButton=new PushButton(mainTab->NewRow(),"start_log");
    39     stopLogButton=new PushButton(mainTab->LastRowLastCol(),"stop_log");
    40     period=new SpinBox(mainTab->NewRow(),"period thread:"," ms",10,1000,1);
     34Loop::Loop(FrameworkManager *parent, string name, int priority)
     35    : Thread(parent, name, priority) {
     36  Tab *mainTab = new Tab(parent->GetTabWidget(), ObjectName());
     37  killButton = new PushButton(mainTab->NewRow(), "kill");
     38  startLogButton = new PushButton(mainTab->NewRow(), "start_log");
     39  stopLogButton = new PushButton(mainTab->LastRowLastCol(), "stop_log");
     40  period = new SpinBox(mainTab->NewRow(), "period thread:", " ms", 10, 1000, 1);
    4141
    42     sinus=new Sinus(parent,"sinus");
    43     sinus->UseDefaultPlot();
     42  sinus = new Sinus(parent, "sinus");
     43  sinus->UseDefaultPlot();
    4444
    45     //1st order law pass filter on raw signal, its parent is the sinus
    46     firstLowPass=new LowPassFilter(sinus,sinus->GetSetupLayout()->NewRow(),"1st order lawpass filter");
    47     sinus->GetPlot()->AddCurve(firstLowPass->Matrix()->Element(0),DataPlot::Blue);//add output of the filter to signal's graph
     45  // 1st order law pass filter on raw signal, its parent is the sinus
     46  firstLowPass = new LowPassFilter(sinus, sinus->GetSetupLayout()->NewRow(),
     47                                   "1st order lawpass filter");
     48  sinus->GetPlot()->AddCurve(
     49      firstLowPass->Matrix()->Element(0),
     50      DataPlot::Blue); // add output of the filter to signal's graph
    4851
    49     //3rd order law pass filter on raw signal, its parent is the sinus
    50     thirdLowPass=new ButterworthLowPass(sinus,sinus->GetSetupLayout()->NewRow(),"3rd order lawpass filter",3);
    51     sinus->GetPlot()->AddCurve(thirdLowPass->Matrix()->Element(0),DataPlot::Yellow);//add output of the filter to signal's graph
     52  // 3rd order law pass filter on raw signal, its parent is the sinus
     53  thirdLowPass = new ButterworthLowPass(
     54      sinus, sinus->GetSetupLayout()->NewRow(), "3rd order lawpass filter", 3);
     55  sinus->GetPlot()->AddCurve(
     56      thirdLowPass->Matrix()->Element(0),
     57      DataPlot::Yellow); // add output of the filter to signal's graph
    5258
    53     //mean filter on raw signal, its parent is the sinus
    54     mean=new MeanFilter(sinus,sinus->GetSetupLayout()->NewRow(),"Mean filter");
    55     sinus->GetPlot()->AddCurve(mean->GetMatrix()->Element(0),DataPlot::Green);//add output of the filter to signal's graph
     59  // mean filter on raw signal, its parent is the sinus
     60  mean =
     61      new MeanFilter(sinus, sinus->GetSetupLayout()->NewRow(), "Mean filter");
     62  sinus->GetPlot()->AddCurve(
     63      mean->GetMatrix()->Element(0),
     64      DataPlot::Green); // add output of the filter to signal's graph
    5665
    57     //mean filter on 1st order law pass filter, its parent is the 1st order law pass filter
    58     meanOnfirstLowPass=new MeanFilter(firstLowPass,sinus->GetSetupLayout()->NewRow(),"Mean filter on 1st order lawpass filter");
    59     sinus->GetPlot()->AddCurve(meanOnfirstLowPass->GetMatrix()->Element(0),DataPlot::Black);//add output of the filter to signal's graph
     66  // mean filter on 1st order law pass filter, its parent is the 1st order law
     67  // pass filter
     68  meanOnfirstLowPass =
     69      new MeanFilter(firstLowPass, sinus->GetSetupLayout()->NewRow(),
     70                     "Mean filter on 1st order lawpass filter");
     71  sinus->GetPlot()->AddCurve(
     72      meanOnfirstLowPass->GetMatrix()->Element(0),
     73      DataPlot::Black); // add output of the filter to signal's graph
    6074
    61     //set ojects to be logged
    62     //as the law pass filters and the mean filters have the sinus as parent, they are automatically logged
    63     parent->AddDeviceToLog(sinus);
     75  // set ojects to be logged
     76  // as the law pass filters and the mean filters have the sinus as parent, they
     77  // are automatically logged
     78  parent->AddDeviceToLog(sinus);
    6479}
    6580
    66 Loop::~Loop() {
     81Loop::~Loop() {}
    6782
     83// main loop of the Thread
     84void Loop::Run(void) {
     85  // warn if changing from primary to secondary mode when in real time
     86  WarnUponSwitches(true);
     87
     88  sinus->Start();
     89
     90  SetPeriodMS(period->Value());
     91
     92  while (1) {
     93    WaitPeriod();
     94
     95    if (period->ValueChanged())
     96      SetPeriodMS(period->Value());
     97
     98    if (killButton->Clicked())
     99      break;
     100    if (startLogButton->Clicked())
     101      getFrameworkManager()->StartLog();
     102    if (stopLogButton->Clicked())
     103      getFrameworkManager()->StopLog();
     104
     105    // nothing more to do
     106    // this is a very simple example
     107
     108    // normaly, we should use results of filters to calculate a control law
     109  }
     110
     111  WarnUponSwitches(false);
    68112}
    69 
    70 //main loop of the Thread
    71 void Loop::Run(void) {
    72     //warn if changing from primary to secondary mode when in real time
    73     WarnUponSwitches(true);
    74 
    75     sinus->Start();
    76 
    77     SetPeriodMS(period->Value());
    78 
    79     while(1) {
    80         WaitPeriod();
    81 
    82         if(period->ValueChanged()) SetPeriodMS(period->Value());
    83 
    84         if(killButton->Clicked()) break;
    85         if(startLogButton->Clicked()) getFrameworkManager()->StartLog();
    86         if(stopLogButton->Clicked()) getFrameworkManager()->StopLog();
    87 
    88         //nothing more to do
    89         //this is a very simple example
    90 
    91         //normaly, we should use results of filters to calculate a control law
    92     }
    93 
    94     WarnUponSwitches(false);
    95 }

  • trunk/demos/Sinus/src/Loop.h

    r14 r16  
    1414#define LOOP_H
    1515
    16 //necessary include, as Thread is a base of our class
     16// necessary include, as Thread is a base of our class
    1717#include <Thread.h>
    1818
    19 //forward declaration for other classes
     19// forward declaration for other classes
    2020namespace flair {
    21     namespace core {
    22        class FrameworkManager;
    23     }
    24     namespace gui {
    25         class PushButton;
    26         class SpinBox;
    27     }
    28     namespace sensor {
    29         class Sinus;
    30     }
    31     namespace filter {
    32         class LowPassFilter;
    33         class ButterworthLowPass;
    34         class MeanFilter;
    35     }
     21namespace core {
     22class FrameworkManager;
     23}
     24namespace gui {
     25class PushButton;
     26class SpinBox;
     27}
     28namespace sensor {
     29class Sinus;
     30}
     31namespace filter {
     32class LowPassFilter;
     33class ButterworthLowPass;
     34class MeanFilter;
     35}
    3636}
    3737
     38class Loop : public flair::core::Thread {
     39public:
     40  /*!
     41  * \brief Constructor
     42  *
     43  * Builds main loop
     44  *
     45  * \param parent the FrameworkManager to use
     46  * \param name object name
     47  * \param priority Thread priority, 51 by default (1:mini, 99:maxi)
     48  */
     49  Loop(flair::core::FrameworkManager *parent, std::string name,
     50       int priority = 51);
    3851
    39 class Loop : public flair::core::Thread
    40 {
    41     public:
    42         /*!
    43         * \brief Constructor
    44         *
    45         * Builds main loop
    46         *
    47         * \param parent the FrameworkManager to use
    48         * \param name object name
    49         * \param priority Thread priority, 51 by default (1:mini, 99:maxi)
    50         */
    51         Loop(flair::core::FrameworkManager* parent,std::string name,int priority=51);
     52  /*!
     53  * \brief Destructor
     54  */
     55  ~Loop();
    5256
    53         /*!
    54         * \brief Destructor
    55         */
    56         ~Loop();
     57private:
     58  void Run(void);
    5759
    58     private:
    59         void Run(void);
    60 
    61         flair::sensor::Sinus *sinus;
    62         flair::filter::LowPassFilter *firstLowPass;
    63         flair::filter::ButterworthLowPass *thirdLowPass;
    64         flair::filter::MeanFilter *mean,*meanOnfirstLowPass;
    65         flair::gui::PushButton *startLogButton,*stopLogButton,*killButton;
    66         flair::gui::SpinBox *period;
     60  flair::sensor::Sinus *sinus;
     61  flair::filter::LowPassFilter *firstLowPass;
     62  flair::filter::ButterworthLowPass *thirdLowPass;
     63  flair::filter::MeanFilter *mean, *meanOnfirstLowPass;
     64  flair::gui::PushButton *startLogButton, *stopLogButton, *killButton;
     65  flair::gui::SpinBox *period;
    6766};
    6867

  • trunk/demos/Sinus/src/MeanFilter.cpp

    r14 r16  
    2222using namespace flair::gui;
    2323
    24 namespace flair { namespace filter {
     24namespace flair {
     25namespace filter {
    2526
    26 MeanFilter::MeanFilter(const IODevice* parent,const LayoutPosition* position,string name): IODevice(parent,name) {
    27     //interface initialisation
    28     groupBox=new GroupBox(position,name);
    29     numberOfElements=new SpinBox(groupBox->NewRow(),"numberOfElements:",1,MAX_NUMBER_OF_ELEMENTS,1);//saturated to MAX_NUMBER_OF_ELEMENTS
     27MeanFilter::MeanFilter(const IODevice *parent, const LayoutPosition *position,
     28                       string name)
     29    : IODevice(parent, name) {
     30  // interface initialisation
     31  groupBox = new GroupBox(position, name);
     32  numberOfElements = new SpinBox(groupBox->NewRow(), "numberOfElements:", 1,
     33                                 MAX_NUMBER_OF_ELEMENTS,
     34                                 1); // saturated to MAX_NUMBER_OF_ELEMENTS
    3035
    31     //init storage
    32     for(int i=0; i<MAX_NUMBER_OF_ELEMENTS; i++) previousValues[i]=0;
     36  // init storage
     37  for (int i = 0; i < MAX_NUMBER_OF_ELEMENTS; i++)
     38    previousValues[i] = 0;
    3339
    34     //1*1 output matrix
    35     cvmatrix_descriptor* desc=new cvmatrix_descriptor(1,1);
    36     desc->SetElementName(0,0,"mean filter");//name will be used for graphs and logs
    37     output=new cvmatrix(this,desc,floatType,name);
     40  // 1*1 output matrix
     41  cvmatrix_descriptor *desc = new cvmatrix_descriptor(1, 1);
     42  desc->SetElementName(0, 0,
     43                       "mean filter"); // name will be used for graphs and logs
     44  output = new cvmatrix(this, desc, floatType, name);
    3845
    39     AddDataToLog(output);
     46  AddDataToLog(output);
    4047}
    4148
    42 MeanFilter::~MeanFilter() {
    43 }
     49MeanFilter::~MeanFilter() {}
    4450
    45 cvmatrix *MeanFilter::GetMatrix() const {
    46     return output;
    47 }
     51cvmatrix *MeanFilter::GetMatrix() const { return output; }
    4852
    49 float MeanFilter::GetValue(void) const {
    50     return output->Value(0,0);
    51 }
     53float MeanFilter::GetValue(void) const { return output->Value(0, 0); }
    5254
    53 //UpdateFrom, where we implement the filter
    54 //this method is automatically called when the parent IODevice calls ProcessUpdate
    55 //in our case it is the sinus or the 1st orde law pass filter
     55// UpdateFrom, where we implement the filter
     56// this method is automatically called when the parent IODevice calls
     57// ProcessUpdate
     58// in our case it is the sinus or the 1st orde law pass filter
    5659//(see in Sinus::Run the call to ProcessUpdate)
    5760void MeanFilter::UpdateFrom(const io_data *data) {
    5861
    59     float result=0;
    60     //get input argument in a cvmatrix
    61     cvmatrix *input=(cvmatrix*)data;
     62  float result = 0;
     63  // get input argument in a cvmatrix
     64  cvmatrix *input = (cvmatrix *)data;
    6265
    63     //simple (and not efficent!) implementation of the filter
    64     previousValues[numberOfElements->Value()-1]=input->Value(0,0);
    65     for(int i=0; i<numberOfElements->Value(); i++) result+=previousValues[i];
    66     for(int i=1; i<numberOfElements->Value(); i++) previousValues[i-1]=previousValues[i];
     66  // simple (and not efficent!) implementation of the filter
     67  previousValues[numberOfElements->Value() - 1] = input->Value(0, 0);
     68  for (int i = 0; i < numberOfElements->Value(); i++)
     69    result += previousValues[i];
     70  for (int i = 1; i < numberOfElements->Value(); i++)
     71    previousValues[i - 1] = previousValues[i];
    6772
    68     //put the result in output matrix
    69     output->SetValue(0,0,result/numberOfElements->Value());
    70     //put corresponding time
    71     output->SetDataTime(data->DataTime());
     73  // put the result in output matrix
     74  output->SetValue(0, 0, result / numberOfElements->Value());
     75  // put corresponding time
     76  output->SetDataTime(data->DataTime());
    7277
    73     //ProcessUpdate is very important
    74     //we must call it after updating the output matrix
    75     //it allows:
    76     // -to save value in the logs
    77     // -to automatically call the next filter UpdateFrom method
    78     ProcessUpdate(output);
     78  // ProcessUpdate is very important
     79  // we must call it after updating the output matrix
     80  // it allows:
     81  // -to save value in the logs
     82  // -to automatically call the next filter UpdateFrom method
     83  ProcessUpdate(output);
    7984}
    80 }// end namespace filter
    81 }// end namespace flair
     85} // end namespace filter
     86} // end namespace flair

  • trunk/demos/Sinus/src/MeanFilter.h

    r14 r16  
    1515#define MEANFILTER_H
    1616
    17 //we fix the max number of elements to compute the mean
    18 //this is a simple example, it should be dynamical
     17// we fix the max number of elements to compute the mean
     18// this is a simple example, it should be dynamical
    1919#define MAX_NUMBER_OF_ELEMENTS 200
    2020
    21 //necessary include, as IODevice is a base of our class
     21// necessary include, as IODevice is a base of our class
    2222#include <IODevice.h>
    2323
    24 //forward declaration for other classes
     24// forward declaration for other classes
    2525namespace flair {
    26     namespace core {
    27         class cvmatrix;
    28     }
    29     namespace gui {
    30         class LayoutPosition;
    31         class GroupBox;
    32         class SpinBox;
    33     }
     26namespace core {
     27class cvmatrix;
     28}
     29namespace gui {
     30class LayoutPosition;
     31class GroupBox;
     32class SpinBox;
     33}
    3434}
    3535
    36 //MeanFilter is a class computing a mean
     36// MeanFilter is a class computing a mean
    3737// based on a parametrizable number of elements
    3838// it derives on IODevice as it as an input and an output
    3939// it is a filter, we extend the namespace
    40 namespace flair { namespace filter {
    41     class MeanFilter : public core::IODevice {
     40namespace flair {
     41namespace filter {
     42class MeanFilter : public core::IODevice {
    4243
    43         public:
    44             /*!
    45             * \brief Constructor
    46             *
    47             * Builds a mean filter. \n
    48             * After calling this function, position will be deleted as it is no longer usefull. \n
    49             *
    50             * \param parent IODevice to use as parent
    51             * \param position where to place settings
    52             * \param name name of the object
    53             */
    54             MeanFilter(const core::IODevice* parent,const gui::LayoutPosition* position,std::string name);
     44public:
     45  /*!
     46  * \brief Constructor
     47  *
     48  * Builds a mean filter. \n
     49  * After calling this function, position will be deleted as it is no longer
     50  *usefull. \n
     51  *
     52  * \param parent IODevice to use as parent
     53  * \param position where to place settings
     54  * \param name name of the object
     55  */
     56  MeanFilter(const core::IODevice *parent, const gui::LayoutPosition *position,
     57             std::string name);
    5558
    56             /*!
    57             * \brief Destructor
    58             */
    59             ~MeanFilter();
     59  /*!
     60  * \brief Destructor
     61  */
     62  ~MeanFilter();
    6063
    61             /*!
    62             * \brief Output matrix
    63             *
    64             * allows to access output matrix, to get signal value or to put it in a graph.  \n
    65             *
    66             * \return pointer to the output matrix
    67             */
    68             core::cvmatrix *GetMatrix(void) const;
     64  /*!
     65  * \brief Output matrix
     66  *
     67  * allows to access output matrix, to get signal value or to put it in a graph.
     68  *\n
     69  *
     70  * \return pointer to the output matrix
     71  */
     72  core::cvmatrix *GetMatrix(void) const;
    6973
    70             /*!
    71             * \brief Value
    72             *
    73             * this method is equivalent to GetMatrix()->Value(0,0)
    74             *
    75             * \return actual mean value
    76             */
    77             float GetValue(void) const;
     74  /*!
     75  * \brief Value
     76  *
     77  * this method is equivalent to GetMatrix()->Value(0,0)
     78  *
     79  * \return actual mean value
     80  */
     81  float GetValue(void) const;
    7882
    79         private:
    80             //UpdateFrom method from base class IODevice
    81             void UpdateFrom(const core::io_data *data);
    82             gui::GroupBox *groupBox;
    83             gui::SpinBox *numberOfElements;
    84             core::cvmatrix *output;
    85             float previousValues[MAX_NUMBER_OF_ELEMENTS];//previous values storage
    86 
    87     };
    88 }// end namespace filter
    89 }// end namespace flair
     83private:
     84  // UpdateFrom method from base class IODevice
     85  void UpdateFrom(const core::io_data *data);
     86  gui::GroupBox *groupBox;
     87  gui::SpinBox *numberOfElements;
     88  core::cvmatrix *output;
     89  float previousValues[MAX_NUMBER_OF_ELEMENTS]; // previous values storage
     90};
     91} // end namespace filter
     92} // end namespace flair
    9093#endif // MEANFILTER_H

  • trunk/demos/Sinus/src/Sinus.cpp

    r14 r16  
    3030using namespace flair::gui;
    3131
    32 namespace flair { namespace sensor {
     32namespace flair {
     33namespace sensor {
    3334
    34 Sinus::Sinus(const FrameworkManager* parent,string name,int priority) : IODevice(parent,name),Thread(parent,name,priority) {
     35Sinus::Sinus(const FrameworkManager *parent, string name, int priority)
     36    : IODevice(parent, name), Thread(parent, name, priority) {
    3537
    36     plot=NULL;
     38  plot = NULL;
    3739
    38     //1*1 output matrix
    39     cvmatrix_descriptor* desc=new cvmatrix_descriptor(1,1);
    40     desc->SetElementName(0,0,"value");//name will be used for graphs and logs
    41     output=new cvmatrix((IODevice*)this,desc,floatType,name);
    42     output->SetValue(0,0,0);
     40  // 1*1 output matrix
     41  cvmatrix_descriptor *desc = new cvmatrix_descriptor(1, 1);
     42  desc->SetElementName(0, 0, "value"); // name will be used for graphs and logs
     43  output = new cvmatrix((IODevice *)this, desc, floatType, name);
     44  output->SetValue(0, 0, 0);
    4345
     46  AddDataToLog(output);
    4447
    45     AddDataToLog(output);
     48  // interface initialisation
     49  mainTab = new Tab(parent->GetTabWidget(), name);
     50  tabWidget = new TabWidget(mainTab->NewRow(), name);
    4651
    47     //interface initialisation
    48     mainTab=new Tab(parent->GetTabWidget(),name);
    49     tabWidget=new TabWidget(mainTab->NewRow(),name);
     52  Tab *settingsTab = new Tab(tabWidget, "Settings");
     53  GroupBox *sinusGroupBox = new GroupBox(settingsTab->NewRow(), name);
     54  frequency = new DoubleSpinBox(sinusGroupBox->NewRow(), "frequence:", " Hz", 0,
     55                                100, 1);
     56  amplitude = new DoubleSpinBox(sinusGroupBox->LastRowLastCol(), "amplitude:",
     57                                0, 10, 1);
     58  offset =
     59      new DoubleSpinBox(sinusGroupBox->LastRowLastCol(), "offset:", 0, 10, 1);
     60  period =
     61      new SpinBox(settingsTab->NewRow(), "period thread:", " ms", 1, 1000, 1);
    5062
    51     Tab* settingsTab=new Tab(tabWidget,"Settings");
    52     GroupBox* sinusGroupBox=new GroupBox(settingsTab->NewRow(),name);
    53     frequency=new DoubleSpinBox(sinusGroupBox->NewRow(),"frequence:"," Hz",0,100,1);
    54     amplitude=new DoubleSpinBox(sinusGroupBox->LastRowLastCol(),"amplitude:",0,10,1);
    55     offset=new DoubleSpinBox(sinusGroupBox->LastRowLastCol(),"offset:",0,10,1);
    56     period=new SpinBox(settingsTab->NewRow(),"period thread:"," ms",1,1000,1);
    57 
    58     setupLayout=new GridLayout(settingsTab->NewRow(),"setup");//layout sui servira à placer des filtres par exemple
     63  setupLayout = new GridLayout(
     64      settingsTab->NewRow(),
     65      "setup"); // layout sui servira à placer des filtres par exemple
    5966}
    60 
    6167
    6268Sinus::~Sinus() {
    6369
    64     SafeStop();
    65     Join();
     70  SafeStop();
     71  Join();
    6672
    67     //main_tab has the FrameworkManager as parent; it will be destroyed when FrameworkManager is destroyed
    68     //it is cleaner to delete it manually, because main_tab is unnecessary when Sinus is deleted
    69     delete mainTab;
     73  // main_tab has the FrameworkManager as parent; it will be destroyed when
     74  // FrameworkManager is destroyed
     75  // it is cleaner to delete it manually, because main_tab is unnecessary when
     76  // Sinus is deleted
     77  delete mainTab;
    7078}
    7179
    7280void Sinus::UseDefaultPlot(void) {
    73     Tab* plotTab=new Tab(tabWidget,"Graph");
    74     plot=new DataPlot1D(plotTab->NewRow(),"Sinus",-10,10);
    75     plot->AddCurve(output->Element(0),DataPlot::Red);
     81  Tab *plotTab = new Tab(tabWidget, "Graph");
     82  plot = new DataPlot1D(plotTab->NewRow(), "Sinus", -10, 10);
     83  plot->AddCurve(output->Element(0), DataPlot::Red);
    7684}
    7785
    78 GridLayout* Sinus::GetSetupLayout(void) const {
    79     return setupLayout;
     86GridLayout *Sinus::GetSetupLayout(void) const { return setupLayout; }
     87
     88DataPlot1D *Sinus::GetPlot(void) const {
     89  if (plot == NULL)
     90    Printf("Sinus::Plot, plot not yet defined, call UseDefaultPlot first\n");
     91  return plot;
    8092}
    8193
    82 DataPlot1D* Sinus::GetPlot(void) const {
    83     if(plot==NULL) Printf("Sinus::Plot, plot not yet defined, call UseDefaultPlot first\n");
    84     return plot;
    85 }
     94cvmatrix *Sinus::GetMatrix(void) const { return output; }
    8695
    87 cvmatrix *Sinus::GetMatrix(void) const {
    88     return output;
    89 }
     96float Sinus::GetValue(void) const { return output->Value(0, 0); }
    9097
    91 float Sinus::GetValue(void) const {
    92     return output->Value(0,0);
    93 }
    94 
    95 //main function, where  we compute the signal
     98// main function, where  we compute the signal
    9699void Sinus::Run(void) {
    97100
    98     SetPeriodMS(period->Value());
     101  SetPeriodMS(period->Value());
    99102
    100     //warn if changing from primary to secondary mode when in real time
    101     WarnUponSwitches(true);
     103  // warn if changing from primary to secondary mode when in real time
     104  WarnUponSwitches(true);
    102105
    103     Time initTime=GetTime();
     106  Time initTime = GetTime();
    104107
    105     while(!ToBeStopped()) {
    106         WaitPeriod();
     108  while (!ToBeStopped()) {
     109    WaitPeriod();
    107110
    108         if(period->ValueChanged()==true) SetPeriodMS(period->Value());
     111    if (period->ValueChanged() == true)
     112      SetPeriodMS(period->Value());
    109113
    110         //compute sinus
    111         Time actualTime=GetTime();
    112         float value=offset->Value()+amplitude->Value()*sinf(2*PI*frequency->Value()*(actualTime-initTime)/1000000000.);//les temps sont en nanosecondes
     114    // compute sinus
     115    Time actualTime = GetTime();
     116    float value =
     117        offset->Value() +
     118        amplitude->Value() *
     119            sinf(2 * PI * frequency->Value() * (actualTime - initTime) /
     120                 1000000000.); // les temps sont en nanosecondes
    113121
    114         //put the result in output matrix
    115         output->SetValue(0,0,value);
    116         //put corresponding time
    117         output->SetDataTime(actualTime);
     122    // put the result in output matrix
     123    output->SetValue(0, 0, value);
     124    // put corresponding time
     125    output->SetDataTime(actualTime);
    118126
    119         //ProcessUpdate is very important
    120         //we must call it after updating the output matrix
    121         //it allows:
    122         // -to save value in the logs
    123         // -to automatically call the next filter UpdateFrom method
    124         ProcessUpdate(output);
    125     }
     127    // ProcessUpdate is very important
     128    // we must call it after updating the output matrix
     129    // it allows:
     130    // -to save value in the logs
     131    // -to automatically call the next filter UpdateFrom method
     132    ProcessUpdate(output);
     133  }
    126134}
    127 }// end namespace sensor
    128 }// end namespace flair
     135} // end namespace sensor
     136} // end namespace flair

  • trunk/demos/Sinus/src/Sinus.h

    r14 r16  
    1515#define SINUS_H
    1616
    17 //necessary includes, as IODevice and Thread are bases of our class
     17// necessary includes, as IODevice and Thread are bases of our class
    1818#include <IODevice.h>
    1919#include <Thread.h>
    2020
    21 //forward declaration for other classes
     21// forward declaration for other classes
    2222namespace flair {
    23     namespace core {
    24         class FrameworkManager;
    25         class cvmatrix;
    26     }
    27     namespace gui {
    28         class Tab;
    29         class TabWidget;
    30         class GridLayout;
    31         class DoubleSpinBox;
    32         class SpinBox;
    33         class DataPlot1D;
    34     }
     23namespace core {
     24class FrameworkManager;
     25class cvmatrix;
     26}
     27namespace gui {
     28class Tab;
     29class TabWidget;
     30class GridLayout;
     31class DoubleSpinBox;
     32class SpinBox;
     33class DataPlot1D;
     34}
    3535}
    3636
    37 //sinus is a class generating a sinus signal
    38 //in this example, it emulates a sensonr, so we extend the namespace sensor
    39 //it derives frome
     37// sinus is a class generating a sinus signal
     38// in this example, it emulates a sensonr, so we extend the namespace sensor
     39// it derives frome
    4040// IODevice: as it has an ouput
    4141// Thread: is it a thread
    42 namespace flair { namespace sensor {
    43     class Sinus : public core::IODevice, public core::Thread {
    44         public:
    45             /*!
    46             * \brief Constructor
    47             *
    48             * Builds a sinus generator
    49             *
    50             * \param parent the FrameworkManager to use
    51             * \param name object name
    52             * \param priority Thread priority, 50 by default (1:mini, 99:maxi)
    53             */
    54             Sinus(const core::FrameworkManager* parent,std::string name,int priority=50);
     42namespace flair {
     43namespace sensor {
     44class Sinus : public core::IODevice, public core::Thread {
     45public:
     46  /*!
     47  * \brief Constructor
     48  *
     49  * Builds a sinus generator
     50  *
     51  * \param parent the FrameworkManager to use
     52  * \param name object name
     53  * \param priority Thread priority, 50 by default (1:mini, 99:maxi)
     54  */
     55  Sinus(const core::FrameworkManager *parent, std::string name,
     56        int priority = 50);
    5557
    56             /*!
    57             * \brief Destructor
    58             */
    59             ~Sinus();
     58  /*!
     59  * \brief Destructor
     60  */
     61  ~Sinus();
    6062
    61             /*!
    62             * \brief Output matrix
    63             *
    64             * allows to access output matrix, to get signal value or to put it in a graph.  \n
    65             *
    66             * \return un pointeur vers la matrice de sortie
    67             */
    68             core::cvmatrix *GetMatrix(void) const;
     63  /*!
     64  * \brief Output matrix
     65  *
     66  * allows to access output matrix, to get signal value or to put it in a graph.
     67  *\n
     68  *
     69  * \return un pointeur vers la matrice de sortie
     70  */
     71  core::cvmatrix *GetMatrix(void) const;
    6972
    70             /*!
    71             * \brief Value
    72             *
    73             * this method is equivalent to GetMatrix()->Value(0,0)
    74             *
    75             * \return actual signal value
    76             */
    77             float GetValue(void) const;
     73  /*!
     74  * \brief Value
     75  *
     76  * this method is equivalent to GetMatrix()->Value(0,0)
     77  *
     78  * \return actual signal value
     79  */
     80  float GetValue(void) const;
    7881
    79             /*!
    80             * \brief Use defautl plot
    81             *
    82             * this method put a graph in a specific tab
    83             *
    84             */
    85             void UseDefaultPlot(void);
     82  /*!
     83  * \brief Use defautl plot
     84  *
     85  * this method put a graph in a specific tab
     86  *
     87  */
     88  void UseDefaultPlot(void);
    8689
    87             /*!
    88             * \brief SetupLayout
    89             *
    90             * this method allows to add other widgets in the sinus tab.
    91             *
    92             * \return the GridLayout
    93             */
    94             gui::GridLayout *GetSetupLayout(void) const;
     90  /*!
     91  * \brief SetupLayout
     92  *
     93  * this method allows to add other widgets in the sinus tab.
     94  *
     95  * \return the GridLayout
     96  */
     97  gui::GridLayout *GetSetupLayout(void) const;
    9598
    96             /*!
    97             * \brief Plot
    98             *
    99             * this method allows to add other curves in the graph
    100             *
    101             * \return the DataPlot1D
    102             */
    103             gui::DataPlot1D *GetPlot(void) const;
     99  /*!
     100  * \brief Plot
     101  *
     102  * this method allows to add other curves in the graph
     103  *
     104  * \return the DataPlot1D
     105  */
     106  gui::DataPlot1D *GetPlot(void) const;
    104107
    105         private:
    106             //UpdateFrom method from base class IODevice
    107             //sinus is like a sensor, so it does not have input; we define an empty method
    108             void UpdateFrom(const core::io_data *data){};
    109             void Run(void);
     108private:
     109  // UpdateFrom method from base class IODevice
     110  // sinus is like a sensor, so it does not have input; we define an empty
     111  // method
     112  void UpdateFrom(const core::io_data *data){};
     113  void Run(void);
    110114
    111             core::cvmatrix *output;
    112             gui::Tab* mainTab;
    113             gui::TabWidget* tabWidget;
    114             gui::DataPlot1D* plot;
    115             gui::DoubleSpinBox *frequency,*amplitude,*offset;
    116             gui::SpinBox *period;
    117             gui::GridLayout* setupLayout;
    118     };
    119 }// end namespace sensor
    120 }// end namespace flair
     115  core::cvmatrix *output;
     116  gui::Tab *mainTab;
     117  gui::TabWidget *tabWidget;
     118  gui::DataPlot1D *plot;
     119  gui::DoubleSpinBox *frequency, *amplitude, *offset;
     120  gui::SpinBox *period;
     121  gui::GridLayout *setupLayout;
     122};
     123} // end namespace sensor
     124} // end namespace flair
    121125#endif // SINUS_H

  • trunk/demos/Sinus/src/main.cpp

    r14 r16  
    1616#include <stdio.h>
    1717
    18 //we use namespace std, to avoid writing for examlpe std::string
     18// we use namespace std, to avoid writing for examlpe std::string
    1919using namespace std;
    20 //idem for TCLAP
     20// idem for TCLAP
    2121using namespace TCLAP;
    22 //idem for flair
     22// idem for flair
    2323using namespace flair::core;
    2424
    25 //global variables for the command line arguments
    26 string logPath;//path for logs
    27 string address;//address for FlairGCS
    28 int port;//port for FlairGCS
    29 string xmlFile;//setup xml file
    30 string name;//name
     25// global variables for the command line arguments
     26string logPath; // path for logs
     27string address; // address for FlairGCS
     28int port; // port for FlairGCS
     29string xmlFile; // setup xml file
     30string name; // name
    3131
    32 void parseOptions(int argc, char** argv);
     32void parseOptions(int argc, char **argv);
    3333
    34 int main(int argc, char* argv[]) {
    35     //get command line arguments (name,port,xml_file,log_path)
    36     parseOptions(argc,argv);
     34int main(int argc, char *argv[]) {
     35  // get command line arguments (name,port,xml_file,log_path)
     36  parseOptions(argc, argv);
    3737
    38     //creation of FrameworkManager
    39     FrameworkManager *manager;
    40     manager= new FrameworkManager(name);
    41     manager->SetupConnection(address,port);
    42     manager->SetupUserInterface(xmlFile);
    43     manager->SetupLogger(logPath);
     38  // creation of FrameworkManager
     39  FrameworkManager *manager;
     40  manager = new FrameworkManager(name);
     41  manager->SetupConnection(address, port);
     42  manager->SetupUserInterface(xmlFile);
     43  manager->SetupLogger(logPath);
    4444
    45     //creation of the
    46     Loop* example=new Loop(manager,"Loop");
     45  // creation of the
     46  Loop *example = new Loop(manager, "Loop");
    4747
    48     //start loop
    49     example->Start();
     48  // start loop
     49  example->Start();
    5050
    51     //wait for loop ends (with kill button)
    52     example->Join();
     51  // wait for loop ends (with kill button)
     52  example->Join();
    5353
    54     delete manager;
     54  delete manager;
    5555}
    5656
     57void parseOptions(int argc, char **argv) {
     58  try {
     59    CmdLine cmd("Command description message", ' ', "0.1");
    5760
    58 void parseOptions(int argc, char** argv) {
    59     try {
    60         CmdLine cmd("Command description message", ' ', "0.1");
     61    // name of the FrameworkManager, Sinus by default
     62    ValueArg<string> nameArg("n", "name", "program name", false, "Sinus",
     63                             "string");
     64    cmd.add(nameArg);
    6165
    62         //name of the FrameworkManager, Sinus by default
    63         ValueArg<string> nameArg("n","name","program name",false,"Sinus","string");
    64         cmd.add( nameArg );
     66    // setup xml file, ./Sinus.xml by default
     67    ValueArg<string> xmlArg("x", "xml", "xml file", false, "./Sinus.xml",
     68                            "string");
     69    cmd.add(xmlArg);
    6570
    66         //setup xml file, ./Sinus.xml by default
    67         ValueArg<string> xmlArg("x","xml","xml file",false,"./Sinus.xml","string");
    68         cmd.add( xmlArg );
     71    // log path, ./ by defaults
     72    ValueArg<string> logArg("l", "log", "log path", false, "/tmp", "string");
     73    cmd.add(logArg);
    6974
    70         //log path, ./ by defaults
    71         ValueArg<string> logArg("l","log","log path",false,"/tmp","string");
    72         cmd.add( logArg );
     75    // address for FlairGCS
     76    ValueArg<string> addressArg("a", "address",
     77                                "address for FlairGCS station sol", false,
     78                                "127.0.0.1", "string");
     79    cmd.add(addressArg);
    7380
    74         //address for FlairGCS
    75         ValueArg<string> addressArg("a","address","address for FlairGCS station sol",false,"127.0.0.1","string");
    76         cmd.add( addressArg );
     81    // port for FlairGCS, 9000 by default
     82    ValueArg<int> portArg("p", "port", "port for FlairGCS", false, 9000, "int");
     83    cmd.add(portArg);
    7784
    78         //port for FlairGCS, 9000 by default
    79         ValueArg<int> portArg("p","port","port for FlairGCS",false,9000,"int");
    80         cmd.add( portArg );
     85    cmd.parse(argc, argv);
    8186
    82         cmd.parse( argc, argv );
     87    // Get the value parsed by each arg.
     88    logPath = logArg.getValue();
     89    port = portArg.getValue();
     90    xmlFile = xmlArg.getValue();
     91    name = nameArg.getValue();
     92    address = addressArg.getValue();
    8393
    84         // Get the value parsed by each arg.
    85         logPath = logArg.getValue();
    86         port=portArg.getValue();
    87         xmlFile = xmlArg.getValue();
    88         name=nameArg.getValue();
    89         address=addressArg.getValue();
    90 
    91     } catch (ArgException &e) { // catch any exceptions
    92         cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
    93     }
     94  } catch (ArgException &e) { // catch any exceptions
     95    cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
     96  }
    9497}

  • trunk/lib/FlairCore/src/ConnectedSocket.h

    r15 r16  
    100100
    101101  //!! See Socket.h for a more generic implementation of network/host endianness
    102   //conversion
     102  // conversion
    103103  virtual uint16_t NetworkToHost16(uint16_t data) = 0;
    104104  virtual uint16_t HostToNetwork16(uint16_t data) = 0;

  • trunk/lib/FlairSensorActuator/src/TargetEthController.h

    r15 r16  
    6262
    6363  bool IsDataFrameReady();
    64   void AcquireAxisData(core::cvmatrix &axis); // responsible for getting the
    65                                               // axis data from the hardware
     64  void AcquireAxisData(core::cvmatrix &axis);     // responsible for getting the
     65                                                  // axis data from the hardware
    6666  void AcquireButtonData(core::cvmatrix &button); // responsible for getting the
    6767                                                  // button data from the

  • trunk/lib/FlairSensorActuator/src/unexported/VrpnClient_impl.h

    r15 r16  
    5151                  flair::core::SerialPort *serialport, uint16_t us_period);
    5252  ~VrpnClient_impl();
    53   void AddTrackable(flair::sensor::VrpnObject *obj); // normal
     53  void AddTrackable(flair::sensor::VrpnObject *obj);    // normal
    5454  void RemoveTrackable(flair::sensor::VrpnObject *obj); // normal
    55   void AddTrackable(VrpnObject_impl *obj, uint8_t id); // xbee
    56   void RemoveTrackable(VrpnObject_impl *obj); // xbee
     55  void AddTrackable(VrpnObject_impl *obj, uint8_t id);  // xbee
     56  void RemoveTrackable(VrpnObject_impl *obj);           // xbee
    5757  void ComputeRotations(flair::core::Vector3D &point);
    5858  void ComputeRotations(flair::core::Quaternion &quat);

  • trunk/lib/FlairSensorActuator/src/unexported/geodesie.h

    r15 r16  
    7171const double phi1_Lambert93 = Deg2Rad(44.0);
    7272const double phi2_Lambert93 = Deg2Rad(49.0); // degres
    73 const double X0_Lambert93 = 700000;  //
    74 const double Y0_Lambert93 = 6600000; //
     73const double X0_Lambert93 = 700000;          //
     74const double Y0_Lambert93 = 6600000;         //
    7575const double n_Lambert93 = 0.7256077650;
    7676const double c_Lambert93 = 11754255.426;

  • 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}

  • trunk/tools/Dbt2csv/src/main.cpp

    r12 r16  
    99
    1010int main(int argc, char *argv[]) {
    11     printf(SVN_REV);
     11  printf(SVN_REV);
    1212
    13     QLocale::setDefault(QLocale::C);
    14     QApplication app(argc, argv);
    15     app.setStyle(new QCleanlooksStyle);
     13  QLocale::setDefault(QLocale::C);
     14  QApplication app(argc, argv);
     15  app.setStyle(new QCleanlooksStyle);
    1616
    17     file_ui* files=new file_ui();
    18     files->connect(files, SIGNAL(finished()), &app, SLOT(quit()));
     17  file_ui *files = new file_ui();
     18  files->connect(files, SIGNAL(finished()), &app, SLOT(quit()));
    1919
    20     QString path= QFileDialog::getExistingDirectory(new QWidget(), "Select a directory",0,0);
    21     QDir directory(path);
    22     QStringList list_files=directory.entryList();
     20  QString path = QFileDialog::getExistingDirectory(new QWidget(),
     21                                                   "Select a directory", 0, 0);
     22  QDir directory(path);
     23  QStringList list_files = directory.entryList();
    2324
    24     for(int i=0;i<list_files.size();i++) {
    25         files->addFile(directory.path()+"/"+list_files.at(i));
    26     }
     25  for (int i = 0; i < list_files.size(); i++) {
     26    files->addFile(directory.path() + "/" + list_files.at(i));
     27  }
    2728
    28     files->endOfFiles();
     29  files->endOfFiles();
    2930
    30     app.exec();
     31  app.exec();
    3132}

  • trunk/tools/FlairGCS/src/XmlWidget.h

    r15 r16  
    3737  bool isExpandable; // true if we can receive extra frame for this widget (ex
    3838                     // combobox, plots, layout)
    39   bool isContainer; // true if it can contain other widget (ex layout),
    40                     // isExpandable is also true in this case
     39  bool isContainer;  // true if it can contain other widget (ex layout),
     40                     // isExpandable is also true in this case
    4141  virtual void SetUptodate(void){};
    4242  virtual void Reset(void){};
Note: See TracChangeset for help on using the changeset viewer.