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

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairSensorActuator/src/TargetEthController.cpp

    r3 r15  
    1212//
    1313//  purpose:    class that gets remote controls through an ethernet connection.
    14 //              Typical use case: a remote control is plugged in a workstation and sends remote control
     14//              Typical use case: a remote control is plugged in a workstation
     15//              and sends remote control
    1516//              data to a distant target (this class) through Wifi//
    1617//
     
    3233using std::string;
    3334
    34 namespace flair
    35 {
    36 namespace sensor
    37 {
    38 
    39 TargetEthController::TargetEthController(const FrameworkManager* parent,string name,uint16_t _port,uint8_t priority) :
    40     TargetController(parent,name,priority),listeningPort(_port),receiveCurrentPosition(0) {
    41     const bool blocking=true;
    42     listeningSocket=new TcpSocket(parent,"TEC_listening_socket",blocking,blocking);
    43     dataSocket=new Socket(parent,"TEC_data_socket",_port+1); //receiving side
     35namespace flair {
     36namespace sensor {
     37
     38TargetEthController::TargetEthController(const FrameworkManager *parent,
     39                                         string name, uint16_t _port,
     40                                         uint8_t priority)
     41    : TargetController(parent, name, priority), listeningPort(_port),
     42      receiveCurrentPosition(0) {
     43  const bool blocking = true;
     44  listeningSocket =
     45      new TcpSocket(parent, "TEC_listening_socket", blocking, blocking);
     46  dataSocket =
     47      new Socket(parent, "TEC_data_socket", _port + 1); // receiving side
    4448}
    4549
    4650TargetEthController::~TargetEthController() {
    47     //We are (currently) the server side. We must ask the client side to initiate tcp connexion closing to avoid the server socket
    48     //to get stuck in TIME_WAIT state
    49     Message msg(32);
    50     if (controlSocket) {
    51         Message cancelAcquisition(sizeof(ControllerAction));
    52         ControllerAction exit=ControllerAction::Exit;
    53         memcpy(cancelAcquisition.buffer,&exit,sizeof(ControllerAction));
    54         controlSocket->SendMessage(cancelAcquisition.buffer,cancelAcquisition.bufferSize);
    55         //We don't expect any more data from the client, we're just waiting for the socket to be closed by the client
    56         controlSocket->RecvMessage(msg.buffer,msg.bufferSize);
    57     }
    58 
    59     //TargetController calls TargetEthController methods in its run
    60     //we must stop the thread now
    61     SafeStop();
    62     Join();
     51  // We are (currently) the server side. We must ask the client side to initiate
     52  // tcp connexion closing to avoid the server socket
     53  // to get stuck in TIME_WAIT state
     54  Message msg(32);
     55  if (controlSocket) {
     56    Message cancelAcquisition(sizeof(ControllerAction));
     57    ControllerAction exit = ControllerAction::Exit;
     58    memcpy(cancelAcquisition.buffer, &exit, sizeof(ControllerAction));
     59    controlSocket->SendMessage(cancelAcquisition.buffer,
     60                               cancelAcquisition.bufferSize);
     61    // We don't expect any more data from the client, we're just waiting for the
     62    // socket to be closed by the client
     63    controlSocket->RecvMessage(msg.buffer, msg.bufferSize);
     64  }
     65
     66  // TargetController calls TargetEthController methods in its run
     67  // we must stop the thread now
     68  SafeStop();
     69  Join();
    6370}
    6471
    6572bool TargetEthController::IsConnected() const {
    66     //TODO
    67 }
    68 
    69 bool TargetEthController::IsDataFrameReady(){
    70     //read up to the last data
    71     ssize_t received;
    72     size_t bytesToReceive=dataFrameSize-receiveCurrentPosition;
    73     bool fullDatagramReceived=false;
    74 
    75     do {
    76         received=dataSocket->RecvMessage(receiveFrameBuffer+receiveCurrentPosition,bytesToReceive,TIME_NONBLOCK);
    77         if (received>0) {
    78             bytesToReceive-=received;
    79             if (bytesToReceive==0) {
    80                 //a full datagram has been read in receiveFrameBuffer
    81                 fullDatagramReceived=true;
    82                 receiveCurrentPosition=0;
    83                 //we swap the data and reception buffers to avoid copy
    84                 char *swapFrameBuffer=dataFrameBuffer;
    85                 dataFrameBuffer=receiveFrameBuffer;
    86                 receiveFrameBuffer=swapFrameBuffer;
    87             }
    88         }
    89     } while (!received<0);
    90 
    91     return fullDatagramReceived;
    92 }
    93 
    94 uint8_t TargetEthController::getByteOrNull(char *buffer,int byte,size_t bufferSize) {
    95     if (byte<bufferSize) return buffer[byte];
    96     else return 0;
    97 }
    98 
    99 uint32_t TargetEthController::charBufferToUint32(char *buffer, size_t bufferSize) {
    100     union {
    101         uint32_t int32;
    102         char byte[4];
    103     } bitField;
    104     if (!IsBigEndian()) {
    105         bitField.byte[0]=getByteOrNull(buffer,3,bufferSize);
    106         bitField.byte[1]=getByteOrNull(buffer,2,bufferSize);
    107         bitField.byte[2]=getByteOrNull(buffer,1,bufferSize);
    108         bitField.byte[3]=getByteOrNull(buffer,0,bufferSize);
    109     } else {
    110         bitField.byte[0]=getByteOrNull(buffer,0,bufferSize);
    111         bitField.byte[1]=getByteOrNull(buffer,1,bufferSize);
    112         bitField.byte[2]=getByteOrNull(buffer,2,bufferSize);
    113         bitField.byte[3]=getByteOrNull(buffer,3,bufferSize);
    114     }
    115     return bitField.int32;
    116 }
    117 
    118 //read _up_to_16_bits_ in a buffer
    119 uint16_t TargetEthController::readBits(uint8_t offsetInBits,uint8_t valueSizeInBits,char *buffer,size_t bufferSize) {
    120     //parameters check
    121     if (valueSizeInBits>16) throw std::range_error("bitfield should be at max 16bits wide");
    122     size_t minBufferSize=(offsetInBits+valueSizeInBits)/8;
    123     if ((offsetInBits+valueSizeInBits)%8!=0) minBufferSize++;
    124     if (bufferSize<minBufferSize) throw std::range_error("buffer too small");
    125     //skip first bytes
    126     size_t bytesToSkip=offsetInBits/8;
    127     buffer+=bytesToSkip;
    128     bufferSize-=bytesToSkip;
    129     offsetInBits-=bytesToSkip*8;
    130     //take care of endianness
    131     uint32_t value=charBufferToUint32(buffer,bufferSize);
    132     value>>=32-offsetInBits-valueSizeInBits;
    133     value&=(1<<valueSizeInBits)-1;
    134     return (uint16_t)value;
     73  // TODO
     74}
     75
     76bool TargetEthController::IsDataFrameReady() {
     77  // read up to the last data
     78  ssize_t received;
     79  size_t bytesToReceive = dataFrameSize - receiveCurrentPosition;
     80  bool fullDatagramReceived = false;
     81
     82  do {
     83    received =
     84        dataSocket->RecvMessage(receiveFrameBuffer + receiveCurrentPosition,
     85                                bytesToReceive, TIME_NONBLOCK);
     86    if (received > 0) {
     87      bytesToReceive -= received;
     88      if (bytesToReceive == 0) {
     89        // a full datagram has been read in receiveFrameBuffer
     90        fullDatagramReceived = true;
     91        receiveCurrentPosition = 0;
     92        // we swap the data and reception buffers to avoid copy
     93        char *swapFrameBuffer = dataFrameBuffer;
     94        dataFrameBuffer = receiveFrameBuffer;
     95        receiveFrameBuffer = swapFrameBuffer;
     96      }
     97    }
     98  } while (!received < 0);
     99
     100  return fullDatagramReceived;
     101}
     102
     103uint8_t TargetEthController::getByteOrNull(char *buffer, int byte,
     104                                           size_t bufferSize) {
     105  if (byte < bufferSize)
     106    return buffer[byte];
     107  else
     108    return 0;
     109}
     110
     111uint32_t TargetEthController::charBufferToUint32(char *buffer,
     112                                                 size_t bufferSize) {
     113  union {
     114    uint32_t int32;
     115    char byte[4];
     116  } bitField;
     117  if (!IsBigEndian()) {
     118    bitField.byte[0] = getByteOrNull(buffer, 3, bufferSize);
     119    bitField.byte[1] = getByteOrNull(buffer, 2, bufferSize);
     120    bitField.byte[2] = getByteOrNull(buffer, 1, bufferSize);
     121    bitField.byte[3] = getByteOrNull(buffer, 0, bufferSize);
     122  } else {
     123    bitField.byte[0] = getByteOrNull(buffer, 0, bufferSize);
     124    bitField.byte[1] = getByteOrNull(buffer, 1, bufferSize);
     125    bitField.byte[2] = getByteOrNull(buffer, 2, bufferSize);
     126    bitField.byte[3] = getByteOrNull(buffer, 3, bufferSize);
     127  }
     128  return bitField.int32;
     129}
     130
     131// read _up_to_16_bits_ in a buffer
     132uint16_t TargetEthController::readBits(uint8_t offsetInBits,
     133                                       uint8_t valueSizeInBits, char *buffer,
     134                                       size_t bufferSize) {
     135  // parameters check
     136  if (valueSizeInBits > 16)
     137    throw std::range_error("bitfield should be at max 16bits wide");
     138  size_t minBufferSize = (offsetInBits + valueSizeInBits) / 8;
     139  if ((offsetInBits + valueSizeInBits) % 8 != 0)
     140    minBufferSize++;
     141  if (bufferSize < minBufferSize)
     142    throw std::range_error("buffer too small");
     143  // skip first bytes
     144  size_t bytesToSkip = offsetInBits / 8;
     145  buffer += bytesToSkip;
     146  bufferSize -= bytesToSkip;
     147  offsetInBits -= bytesToSkip * 8;
     148  // take care of endianness
     149  uint32_t value = charBufferToUint32(buffer, bufferSize);
     150  value >>= 32 - offsetInBits - valueSizeInBits;
     151  value &= (1 << valueSizeInBits) - 1;
     152  return (uint16_t)value;
    135153}
    136154
    137155void TargetEthController::AcquireAxisData(core::cvmatrix &axis) {
    138     axis.GetMutex();
    139 //char testFrameBuffer[3]={(char)0x09,(char)0x59,(char)0xB8};
    140     for (unsigned int i=0;i<axisNumber;i++) {
    141         uint16_t rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,dataFrameBuffer,dataFrameSize);
    142 //        uint16_t rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,testFrameBuffer);
    143         uint16_t scale=1<<(bitsPerAxis-1);
    144 //Thread::Info("RawAxisValue=%d, scale=%d => scaled rawValue=%d, float value)%f\n",rawAxisValue,scale,rawAxisValue-scale,(rawAxisValue-scale)/(float)scale);
    145         axis.SetValueNoMutex(i,0,(rawAxisValue-scale)/(float)scale);
    146     }
    147     axis.ReleaseMutex();
     156  axis.GetMutex();
     157  // char testFrameBuffer[3]={(char)0x09,(char)0x59,(char)0xB8};
     158  for (unsigned int i = 0; i < axisNumber; i++) {
     159    uint16_t rawAxisValue =
     160        readBits(i * bitsPerAxis, bitsPerAxis, dataFrameBuffer, dataFrameSize);
     161    //        uint16_t
     162    //        rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,testFrameBuffer);
     163    uint16_t scale = 1 << (bitsPerAxis - 1);
     164    // Thread::Info("RawAxisValue=%d, scale=%d => scaled rawValue=%d, float
     165    // value)%f\n",rawAxisValue,scale,rawAxisValue-scale,(rawAxisValue-scale)/(float)scale);
     166    axis.SetValueNoMutex(i, 0, (rawAxisValue - scale) / (float)scale);
     167  }
     168  axis.ReleaseMutex();
    148169}
    149170
    150171void TargetEthController::AcquireButtonData(core::cvmatrix &button) {
    151     uint8_t buttonValue;
    152     int currentButton=0;
    153     button.GetMutex();
    154     /*
    155     for (unsigned int i=0;i<buttonNumber;i++) {
    156         memcpy(&buttonValue,buttonOffset+i*sizeof(bool),sizeof(bool));
    157         dataSocket->NetworkToHost((char*)&buttonValue,sizeof(bool));
    158         button.SetValueNoMutex(i,0,buttonValue);
    159 //        if (buttonValue) Thread::Info("Debug: button '%s' pressed\n", GetButtonName(i).c_str());
    160     }*/
    161     for (unsigned int i=0;i<buttonNumber/8;i++) {
    162         memcpy(&buttonValue,dataFrameBuffer+buttonOffset+i*sizeof(uint8_t),sizeof(uint8_t));
    163 //        dataSocket->NetworkToHost((char*)&buttonValue,sizeof(uint8_t));
    164         for(unsigned int j=0;j<8;j++) {
    165             button.SetValueNoMutex(currentButton,0,(buttonValue>>j)&0x01);
    166             currentButton++;
    167         }
    168     }
    169     button.ReleaseMutex();
    170 }
    171 
    172 string TargetEthController::GetAxisName(unsigned int axisId) const{
    173     //TODO: should throw an exception if axisName==NULL or axisId>axisNumber
    174     return axisName[axisId];
    175 }
    176 
    177 string TargetEthController::GetButtonName(unsigned int buttonId) const{
    178     //TODO: should throw an exception if buttonName==NULL or buttonId>buttonNumber
    179     return buttonName[buttonId];
     172  uint8_t buttonValue;
     173  int currentButton = 0;
     174  button.GetMutex();
     175  /*
     176  for (unsigned int i=0;i<buttonNumber;i++) {
     177      memcpy(&buttonValue,buttonOffset+i*sizeof(bool),sizeof(bool));
     178      dataSocket->NetworkToHost((char*)&buttonValue,sizeof(bool));
     179      button.SetValueNoMutex(i,0,buttonValue);
     180//        if (buttonValue) Thread::Info("Debug: button '%s' pressed\n",
     181GetButtonName(i).c_str());
     182  }*/
     183  for (unsigned int i = 0; i < buttonNumber / 8; i++) {
     184    memcpy(&buttonValue, dataFrameBuffer + buttonOffset + i * sizeof(uint8_t),
     185           sizeof(uint8_t));
     186    //        dataSocket->NetworkToHost((char*)&buttonValue,sizeof(uint8_t));
     187    for (unsigned int j = 0; j < 8; j++) {
     188      button.SetValueNoMutex(currentButton, 0, (buttonValue >> j) & 0x01);
     189      currentButton++;
     190    }
     191  }
     192  button.ReleaseMutex();
     193}
     194
     195string TargetEthController::GetAxisName(unsigned int axisId) const {
     196  // TODO: should throw an exception if axisName==NULL or axisId>axisNumber
     197  return axisName[axisId];
     198}
     199
     200string TargetEthController::GetButtonName(unsigned int buttonId) const {
     201  // TODO: should throw an exception if buttonName==NULL or
     202  // buttonId>buttonNumber
     203  return buttonName[buttonId];
    180204}
    181205
    182206bool TargetEthController::ProcessMessage(Message *msg) {
    183     return !(controlSocket->SendMessage(msg->buffer,msg->bufferSize,0)<0);
    184 }
    185 
    186 bool TargetEthController::IsControllerActionSupported(ControllerAction action) const {
    187     //TODO: here we should ask the remote side (host). Probably through the control socket
    188     switch(action) {
    189     case ControllerAction::SetLedOn:
    190         return true;
    191     case ControllerAction::SetLedOff:
    192         return true;
    193     case ControllerAction::Rumble:
    194         return true;
    195     case ControllerAction::FlashLed:
    196         return true;
    197     default:
     207  return !(controlSocket->SendMessage(msg->buffer, msg->bufferSize, 0) < 0);
     208}
     209
     210bool TargetEthController::IsControllerActionSupported(
     211    ControllerAction action) const {
     212  // TODO: here we should ask the remote side (host). Probably through the
     213  // control socket
     214  switch (action) {
     215  case ControllerAction::SetLedOn:
     216    return true;
     217  case ControllerAction::SetLedOff:
     218    return true;
     219  case ControllerAction::Rumble:
     220    return true;
     221  case ControllerAction::FlashLed:
     222    return true;
     223  default:
     224    return false;
     225  }
     226}
     227
     228bool TargetEthController::ControllerInitialization() {
     229  char message[1024];
     230  ssize_t received;
     231  bool connected = false;
     232  bool mustReadMore;
     233
     234  listeningSocket->Listen(listeningPort);
     235  Thread::Info("Debug: Listening to port %d\n", listeningPort);
     236  // accept incoming connection
     237  bool connectionAccepted = false;
     238  while (!connectionAccepted) {
     239    controlSocket = listeningSocket->Accept(10);
     240    if (controlSocket == nullptr) {
     241      // Timeout (or error btw)
     242      if (ToBeStopped())
    198243        return false;
    199     }
    200 }
    201 
    202 bool TargetEthController::ControllerInitialization() {
    203     char message[1024];
    204     ssize_t received;
    205     bool connected=false;
    206     bool mustReadMore;
    207 
    208     listeningSocket->Listen(listeningPort);
    209     Thread::Info("Debug: Listening to port %d\n",listeningPort);
    210                  //accept incoming connection
    211     bool connectionAccepted=false;
    212     while (!connectionAccepted) {
    213         controlSocket=listeningSocket->Accept(10);
    214         if (controlSocket==nullptr) {
    215             //Timeout (or error btw)
    216             if (ToBeStopped()) return false;
    217         } else connectionAccepted=true;
    218     }
    219     Thread::Info("Debug: Connexion accepted\n");
    220 
    221     //get axis stuff
    222     bool axisNumberRead=false;
    223     while (!axisNumberRead) {
    224         try {
    225             axisNumber=controlSocket->ReadUInt32(10);
    226             //Thread::Info("Debug: axisNumber %d\n", axisNumber);
    227             axisNumberRead=true;
    228         }
    229         catch (std::runtime_error e) {
    230             //timeout
    231             if (ToBeStopped()) return false;
    232         }
    233     }
    234     bool bitsPerAxisRead=false;
    235     while (!bitsPerAxisRead) {
    236         try {
    237             bitsPerAxis=controlSocket->ReadUInt32(10);
    238             //Thread::Info("Debug: bits per axis %d\n", bitsPerAxis);
    239             bitsPerAxisRead=true;
    240         }
    241         catch (std::runtime_error e) {
    242             //timeout
    243             if (ToBeStopped()) return false;
    244         }
    245     }
    246     axisName=new string[axisNumber];
    247     for (unsigned int i=0;i<axisNumber;i++) {
    248         //read string size
    249         int stringSize;
    250         bool stringSizeRead=false;
    251         while (!stringSizeRead) {
    252             try {
    253                 stringSize=controlSocket->ReadUInt32(10);
    254                 stringSizeRead=true;
    255             }
    256             catch (std::runtime_error e) {
    257                 //timeout
    258                 if (ToBeStopped()) return false;
    259             }
    260         }
    261         //read string
    262         bool axisNameRead=false;
    263         while (!axisNameRead) {
    264             try {
    265                 axisName[i]=controlSocket->ReadString(stringSize,10);
    266                 axisNameRead=true;
    267             }
    268             catch (std::runtime_error e) {
    269                 //timeout
    270                 if (ToBeStopped()) return false;
    271             }
    272         }
    273         //Thread::Info("Debug: axisName for axis %d %s\n", i, axisName[i].c_str());
    274     }
    275 
    276     //get button stuff
    277     bool buttonNumberRead=false;
    278     while (!buttonNumberRead) {
    279         try {
    280             buttonNumber=controlSocket->ReadUInt32(10);
    281             buttonNumberRead=true;
    282         }
    283         catch (std::runtime_error e) {
    284             //timeout
    285             if (ToBeStopped()) return false;
    286         }
    287     }
    288     //Thread::Info("Debug: buttonNumber %d\n", buttonNumber);
    289     buttonName=new string[buttonNumber];
    290     for (unsigned int i=0;i<buttonNumber;i++) {
    291         //read string size
    292         int stringSize;
    293         bool stringSizeRead=false;
    294         while (!stringSizeRead) {
    295             try {
    296                 stringSize=controlSocket->ReadUInt32(10);
    297                 stringSizeRead=true;
    298             }
    299             catch (std::runtime_error e) {
    300                 //timeout
    301                 if (ToBeStopped()) return false;
    302             }
    303         }
    304         //read string
    305         bool buttonNameRead=false;
    306         while (!buttonNameRead) {
    307             try {
    308                 buttonName[i]=controlSocket->ReadString(stringSize,10);
    309                 buttonNameRead=true;
    310             }
    311             catch (std::runtime_error e) {
    312                 //timeout
    313                 if (ToBeStopped()) return false;
    314             }
    315         }
    316         //Thread::Info("Debug: buttonName for button %d %s\n", i, buttonName[i].c_str());
    317     }
    318 
    319 //    dataFrameSize=axisNumber*sizeof(float)+buttonNumber/8*sizeof(uint8_t);
    320     buttonOffset=(axisNumber*bitsPerAxis)/8;
    321     if ((axisNumber*bitsPerAxis)%8!=0) buttonOffset++;
    322     dataFrameSize=buttonOffset+(buttonNumber/8)*sizeof(uint8_t);
    323     if ((buttonNumber%8)!=0) dataFrameSize++;
    324     dataFrameBuffer=new char[dataFrameSize];
    325     receiveFrameBuffer=new char[dataFrameSize];
    326 
    327     Thread::Info("Controller connected with host side\n");
    328     if(buttonNumber%8!=0) Thread::Err("Button number is not multiple of 8\n");
    329     return true;
     244    } else
     245      connectionAccepted = true;
     246  }
     247  Thread::Info("Debug: Connexion accepted\n");
     248
     249  // get axis stuff
     250  bool axisNumberRead = false;
     251  while (!axisNumberRead) {
     252    try {
     253      axisNumber = controlSocket->ReadUInt32(10);
     254      // Thread::Info("Debug: axisNumber %d\n", axisNumber);
     255      axisNumberRead = true;
     256    } catch (std::runtime_error e) {
     257      // timeout
     258      if (ToBeStopped())
     259        return false;
     260    }
     261  }
     262  bool bitsPerAxisRead = false;
     263  while (!bitsPerAxisRead) {
     264    try {
     265      bitsPerAxis = controlSocket->ReadUInt32(10);
     266      // Thread::Info("Debug: bits per axis %d\n", bitsPerAxis);
     267      bitsPerAxisRead = true;
     268    } catch (std::runtime_error e) {
     269      // timeout
     270      if (ToBeStopped())
     271        return false;
     272    }
     273  }
     274  axisName = new string[axisNumber];
     275  for (unsigned int i = 0; i < axisNumber; i++) {
     276    // read string size
     277    int stringSize;
     278    bool stringSizeRead = false;
     279    while (!stringSizeRead) {
     280      try {
     281        stringSize = controlSocket->ReadUInt32(10);
     282        stringSizeRead = true;
     283      } catch (std::runtime_error e) {
     284        // timeout
     285        if (ToBeStopped())
     286          return false;
     287      }
     288    }
     289    // read string
     290    bool axisNameRead = false;
     291    while (!axisNameRead) {
     292      try {
     293        axisName[i] = controlSocket->ReadString(stringSize, 10);
     294        axisNameRead = true;
     295      } catch (std::runtime_error e) {
     296        // timeout
     297        if (ToBeStopped())
     298          return false;
     299      }
     300    }
     301    // Thread::Info("Debug: axisName for axis %d %s\n", i, axisName[i].c_str());
     302  }
     303
     304  // get button stuff
     305  bool buttonNumberRead = false;
     306  while (!buttonNumberRead) {
     307    try {
     308      buttonNumber = controlSocket->ReadUInt32(10);
     309      buttonNumberRead = true;
     310    } catch (std::runtime_error e) {
     311      // timeout
     312      if (ToBeStopped())
     313        return false;
     314    }
     315  }
     316  // Thread::Info("Debug: buttonNumber %d\n", buttonNumber);
     317  buttonName = new string[buttonNumber];
     318  for (unsigned int i = 0; i < buttonNumber; i++) {
     319    // read string size
     320    int stringSize;
     321    bool stringSizeRead = false;
     322    while (!stringSizeRead) {
     323      try {
     324        stringSize = controlSocket->ReadUInt32(10);
     325        stringSizeRead = true;
     326      } catch (std::runtime_error e) {
     327        // timeout
     328        if (ToBeStopped())
     329          return false;
     330      }
     331    }
     332    // read string
     333    bool buttonNameRead = false;
     334    while (!buttonNameRead) {
     335      try {
     336        buttonName[i] = controlSocket->ReadString(stringSize, 10);
     337        buttonNameRead = true;
     338      } catch (std::runtime_error e) {
     339        // timeout
     340        if (ToBeStopped())
     341          return false;
     342      }
     343    }
     344    // Thread::Info("Debug: buttonName for button %d %s\n", i,
     345    // buttonName[i].c_str());
     346  }
     347
     348  //    dataFrameSize=axisNumber*sizeof(float)+buttonNumber/8*sizeof(uint8_t);
     349  buttonOffset = (axisNumber * bitsPerAxis) / 8;
     350  if ((axisNumber * bitsPerAxis) % 8 != 0)
     351    buttonOffset++;
     352  dataFrameSize = buttonOffset + (buttonNumber / 8) * sizeof(uint8_t);
     353  if ((buttonNumber % 8) != 0)
     354    dataFrameSize++;
     355  dataFrameBuffer = new char[dataFrameSize];
     356  receiveFrameBuffer = new char[dataFrameSize];
     357
     358  Thread::Info("Controller connected with host side\n");
     359  if (buttonNumber % 8 != 0)
     360    Thread::Err("Button number is not multiple of 8\n");
     361  return true;
    330362}
    331363
Note: See TracChangeset for help on using the changeset viewer.