Changeset 15 in flair-src for trunk/lib/FlairSensorActuator/src/TargetEthController.cpp

Timestamp:

04/08/16 15:40:57 (8 years ago)

Author:

Bayard Gildas

Message:

sources reformatted with flair-format-dir script

File:

• trunk/lib/FlairSensorActuator/src/TargetEthController.cpp (modified) (2 diffs)

trunk/lib/FlairSensorActuator/src/TargetEthController.cpp

@@ -12 +12 @@
 //
 //  purpose:    class that gets remote controls through an ethernet connection.
-//              Typical use case: a remote control is plugged in a workstation and sends remote control
+//              Typical use case: a remote control is plugged in a workstation
+//              and sends remote control
 //              data to a distant target (this class) through Wifi//
 //
@@ -32 +33 @@
 using std::string;
 
-namespace flair
-{
-namespace sensor
-{
-
-TargetEthController::TargetEthController(const FrameworkManager* parent,string name,uint16_t _port,uint8_t priority) :
-    TargetController(parent,name,priority),listeningPort(_port),receiveCurrentPosition(0) {
-    const bool blocking=true;
-    listeningSocket=new TcpSocket(parent,"TEC_listening_socket",blocking,blocking);
-    dataSocket=new Socket(parent,"TEC_data_socket",_port+1); //receiving side
+namespace flair {
+namespace sensor {
+
+TargetEthController::TargetEthController(const FrameworkManager *parent,
+                                         string name, uint16_t _port,
+                                         uint8_t priority)
+    : TargetController(parent, name, priority), listeningPort(_port),
+      receiveCurrentPosition(0) {
+  const bool blocking = true;
+  listeningSocket =
+      new TcpSocket(parent, "TEC_listening_socket", blocking, blocking);
+  dataSocket =
+      new Socket(parent, "TEC_data_socket", _port + 1); // receiving side
 }
 
 TargetEthController::~TargetEthController() {
-    //We are (currently) the server side. We must ask the client side to initiate tcp connexion closing to avoid the server socket
-    //to get stuck in TIME_WAIT state
-    Message msg(32);
-    if (controlSocket) {
-        Message cancelAcquisition(sizeof(ControllerAction));
-        ControllerAction exit=ControllerAction::Exit;
-        memcpy(cancelAcquisition.buffer,&exit,sizeof(ControllerAction));
-        controlSocket->SendMessage(cancelAcquisition.buffer,cancelAcquisition.bufferSize);
-        //We don't expect any more data from the client, we're just waiting for the socket to be closed by the client
-        controlSocket->RecvMessage(msg.buffer,msg.bufferSize);
-    }
-
-    //TargetController calls TargetEthController methods in its run
-    //we must stop the thread now
-    SafeStop();
-    Join();
+  // We are (currently) the server side. We must ask the client side to initiate
+  // tcp connexion closing to avoid the server socket
+  // to get stuck in TIME_WAIT state
+  Message msg(32);
+  if (controlSocket) {
+    Message cancelAcquisition(sizeof(ControllerAction));
+    ControllerAction exit = ControllerAction::Exit;
+    memcpy(cancelAcquisition.buffer, &exit, sizeof(ControllerAction));
+    controlSocket->SendMessage(cancelAcquisition.buffer,
+                               cancelAcquisition.bufferSize);
+    // We don't expect any more data from the client, we're just waiting for the
+    // socket to be closed by the client
+    controlSocket->RecvMessage(msg.buffer, msg.bufferSize);
+  }
+
+  // TargetController calls TargetEthController methods in its run
+  // we must stop the thread now
+  SafeStop();
+  Join();
 }
 
 bool TargetEthController::IsConnected() const {
-    //TODO
-}
-
-bool TargetEthController::IsDataFrameReady(){
-    //read up to the last data
-    ssize_t received;
-    size_t bytesToReceive=dataFrameSize-receiveCurrentPosition;
-    bool fullDatagramReceived=false;
-
-    do {
-        received=dataSocket->RecvMessage(receiveFrameBuffer+receiveCurrentPosition,bytesToReceive,TIME_NONBLOCK);
-        if (received>0) {
-            bytesToReceive-=received;
-            if (bytesToReceive==0) {
-                //a full datagram has been read in receiveFrameBuffer
-                fullDatagramReceived=true;
-                receiveCurrentPosition=0;
-                //we swap the data and reception buffers to avoid copy
-                char *swapFrameBuffer=dataFrameBuffer;
-                dataFrameBuffer=receiveFrameBuffer;
-                receiveFrameBuffer=swapFrameBuffer;
-            }
-        }
-    } while (!received<0);
-
-    return fullDatagramReceived;
-}
-
-uint8_t TargetEthController::getByteOrNull(char *buffer,int byte,size_t bufferSize) {
-    if (byte<bufferSize) return buffer[byte];
-    else return 0;
-}
-
-uint32_t TargetEthController::charBufferToUint32(char *buffer, size_t bufferSize) {
-    union {
-        uint32_t int32;
-        char byte[4];
-    } bitField;
-    if (!IsBigEndian()) {
-        bitField.byte[0]=getByteOrNull(buffer,3,bufferSize);
-        bitField.byte[1]=getByteOrNull(buffer,2,bufferSize);
-        bitField.byte[2]=getByteOrNull(buffer,1,bufferSize);
-        bitField.byte[3]=getByteOrNull(buffer,0,bufferSize);
-    } else {
-        bitField.byte[0]=getByteOrNull(buffer,0,bufferSize);
-        bitField.byte[1]=getByteOrNull(buffer,1,bufferSize);
-        bitField.byte[2]=getByteOrNull(buffer,2,bufferSize);
-        bitField.byte[3]=getByteOrNull(buffer,3,bufferSize);
-    }
-    return bitField.int32;
-}
-
-//read _up_to_16_bits_ in a buffer
-uint16_t TargetEthController::readBits(uint8_t offsetInBits,uint8_t valueSizeInBits,char *buffer,size_t bufferSize) {
-    //parameters check
-    if (valueSizeInBits>16) throw std::range_error("bitfield should be at max 16bits wide");
-    size_t minBufferSize=(offsetInBits+valueSizeInBits)/8;
-    if ((offsetInBits+valueSizeInBits)%8!=0) minBufferSize++;
-    if (bufferSize<minBufferSize) throw std::range_error("buffer too small");
-    //skip first bytes
-    size_t bytesToSkip=offsetInBits/8;
-    buffer+=bytesToSkip;
-    bufferSize-=bytesToSkip;
-    offsetInBits-=bytesToSkip*8;
-    //take care of endianness
-    uint32_t value=charBufferToUint32(buffer,bufferSize);
-    value>>=32-offsetInBits-valueSizeInBits;
-    value&=(1<<valueSizeInBits)-1;
-    return (uint16_t)value;
+  // TODO
+}
+
+bool TargetEthController::IsDataFrameReady() {
+  // read up to the last data
+  ssize_t received;
+  size_t bytesToReceive = dataFrameSize - receiveCurrentPosition;
+  bool fullDatagramReceived = false;
+
+  do {
+    received =
+        dataSocket->RecvMessage(receiveFrameBuffer + receiveCurrentPosition,
+                                bytesToReceive, TIME_NONBLOCK);
+    if (received > 0) {
+      bytesToReceive -= received;
+      if (bytesToReceive == 0) {
+        // a full datagram has been read in receiveFrameBuffer
+        fullDatagramReceived = true;
+        receiveCurrentPosition = 0;
+        // we swap the data and reception buffers to avoid copy
+        char *swapFrameBuffer = dataFrameBuffer;
+        dataFrameBuffer = receiveFrameBuffer;
+        receiveFrameBuffer = swapFrameBuffer;
+      }
+    }
+  } while (!received < 0);
+
+  return fullDatagramReceived;
+}
+
+uint8_t TargetEthController::getByteOrNull(char *buffer, int byte,
+                                           size_t bufferSize) {
+  if (byte < bufferSize)
+    return buffer[byte];
+  else
+    return 0;
+}
+
+uint32_t TargetEthController::charBufferToUint32(char *buffer,
+                                                 size_t bufferSize) {
+  union {
+    uint32_t int32;
+    char byte[4];
+  } bitField;
+  if (!IsBigEndian()) {
+    bitField.byte[0] = getByteOrNull(buffer, 3, bufferSize);
+    bitField.byte[1] = getByteOrNull(buffer, 2, bufferSize);
+    bitField.byte[2] = getByteOrNull(buffer, 1, bufferSize);
+    bitField.byte[3] = getByteOrNull(buffer, 0, bufferSize);
+  } else {
+    bitField.byte[0] = getByteOrNull(buffer, 0, bufferSize);
+    bitField.byte[1] = getByteOrNull(buffer, 1, bufferSize);
+    bitField.byte[2] = getByteOrNull(buffer, 2, bufferSize);
+    bitField.byte[3] = getByteOrNull(buffer, 3, bufferSize);
+  }
+  return bitField.int32;
+}
+
+// read _up_to_16_bits_ in a buffer
+uint16_t TargetEthController::readBits(uint8_t offsetInBits,
+                                       uint8_t valueSizeInBits, char *buffer,
+                                       size_t bufferSize) {
+  // parameters check
+  if (valueSizeInBits > 16)
+    throw std::range_error("bitfield should be at max 16bits wide");
+  size_t minBufferSize = (offsetInBits + valueSizeInBits) / 8;
+  if ((offsetInBits + valueSizeInBits) % 8 != 0)
+    minBufferSize++;
+  if (bufferSize < minBufferSize)
+    throw std::range_error("buffer too small");
+  // skip first bytes
+  size_t bytesToSkip = offsetInBits / 8;
+  buffer += bytesToSkip;
+  bufferSize -= bytesToSkip;
+  offsetInBits -= bytesToSkip * 8;
+  // take care of endianness
+  uint32_t value = charBufferToUint32(buffer, bufferSize);
+  value >>= 32 - offsetInBits - valueSizeInBits;
+  value &= (1 << valueSizeInBits) - 1;
+  return (uint16_t)value;
 }
 
 void TargetEthController::AcquireAxisData(core::cvmatrix &axis) {
-    axis.GetMutex();
-//char testFrameBuffer[3]={(char)0x09,(char)0x59,(char)0xB8};
-    for (unsigned int i=0;i<axisNumber;i++) {
-        uint16_t rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,dataFrameBuffer,dataFrameSize);
-//        uint16_t rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,testFrameBuffer);
-        uint16_t scale=1<<(bitsPerAxis-1);
-//Thread::Info("RawAxisValue=%d, scale=%d => scaled rawValue=%d, float value)%f\n",rawAxisValue,scale,rawAxisValue-scale,(rawAxisValue-scale)/(float)scale);
-        axis.SetValueNoMutex(i,0,(rawAxisValue-scale)/(float)scale);
-    }
-    axis.ReleaseMutex();
+  axis.GetMutex();
+  // char testFrameBuffer[3]={(char)0x09,(char)0x59,(char)0xB8};
+  for (unsigned int i = 0; i < axisNumber; i++) {
+    uint16_t rawAxisValue =
+        readBits(i * bitsPerAxis, bitsPerAxis, dataFrameBuffer, dataFrameSize);
+    //        uint16_t
+    //        rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,testFrameBuffer);
+    uint16_t scale = 1 << (bitsPerAxis - 1);
+    // Thread::Info("RawAxisValue=%d, scale=%d => scaled rawValue=%d, float
+    // value)%f\n",rawAxisValue,scale,rawAxisValue-scale,(rawAxisValue-scale)/(float)scale);
+    axis.SetValueNoMutex(i, 0, (rawAxisValue - scale) / (float)scale);
+  }
+  axis.ReleaseMutex();
 }
 
 void TargetEthController::AcquireButtonData(core::cvmatrix &button) {
-    uint8_t buttonValue;
-    int currentButton=0;
-    button.GetMutex();
-    /*
-    for (unsigned int i=0;i<buttonNumber;i++) {
-        memcpy(&buttonValue,buttonOffset+i*sizeof(bool),sizeof(bool));
-        dataSocket->NetworkToHost((char*)&buttonValue,sizeof(bool));
-        button.SetValueNoMutex(i,0,buttonValue);
-//        if (buttonValue) Thread::Info("Debug: button '%s' pressed\n", GetButtonName(i).c_str());
-    }*/
-    for (unsigned int i=0;i<buttonNumber/8;i++) {
-        memcpy(&buttonValue,dataFrameBuffer+buttonOffset+i*sizeof(uint8_t),sizeof(uint8_t));
-//        dataSocket->NetworkToHost((char*)&buttonValue,sizeof(uint8_t));
-        for(unsigned int j=0;j<8;j++) {
-            button.SetValueNoMutex(currentButton,0,(buttonValue>>j)&0x01);
-            currentButton++;
-        }
-    }
-    button.ReleaseMutex();
-}
-
-string TargetEthController::GetAxisName(unsigned int axisId) const{
-    //TODO: should throw an exception if axisName==NULL or axisId>axisNumber
-    return axisName[axisId];
-}
-
-string TargetEthController::GetButtonName(unsigned int buttonId) const{
-    //TODO: should throw an exception if buttonName==NULL or buttonId>buttonNumber
-    return buttonName[buttonId];
+  uint8_t buttonValue;
+  int currentButton = 0;
+  button.GetMutex();
+  /*
+  for (unsigned int i=0;i<buttonNumber;i++) {
+      memcpy(&buttonValue,buttonOffset+i*sizeof(bool),sizeof(bool));
+      dataSocket->NetworkToHost((char*)&buttonValue,sizeof(bool));
+      button.SetValueNoMutex(i,0,buttonValue);
+//        if (buttonValue) Thread::Info("Debug: button '%s' pressed\n",
+GetButtonName(i).c_str());
+  }*/
+  for (unsigned int i = 0; i < buttonNumber / 8; i++) {
+    memcpy(&buttonValue, dataFrameBuffer + buttonOffset + i * sizeof(uint8_t),
+           sizeof(uint8_t));
+    //        dataSocket->NetworkToHost((char*)&buttonValue,sizeof(uint8_t));
+    for (unsigned int j = 0; j < 8; j++) {
+      button.SetValueNoMutex(currentButton, 0, (buttonValue >> j) & 0x01);
+      currentButton++;
+    }
+  }
+  button.ReleaseMutex();
+}
+
+string TargetEthController::GetAxisName(unsigned int axisId) const {
+  // TODO: should throw an exception if axisName==NULL or axisId>axisNumber
+  return axisName[axisId];
+}
+
+string TargetEthController::GetButtonName(unsigned int buttonId) const {
+  // TODO: should throw an exception if buttonName==NULL or
+  // buttonId>buttonNumber
+  return buttonName[buttonId];
 }
 
 bool TargetEthController::ProcessMessage(Message *msg) {
-    return !(controlSocket->SendMessage(msg->buffer,msg->bufferSize,0)<0);
-}
-
-bool TargetEthController::IsControllerActionSupported(ControllerAction action) const {
-    //TODO: here we should ask the remote side (host). Probably through the control socket
-    switch(action) {
-    case ControllerAction::SetLedOn:
-        return true;
-    case ControllerAction::SetLedOff:
-        return true;
-    case ControllerAction::Rumble:
-        return true;
-    case ControllerAction::FlashLed:
-        return true;
-    default:
+  return !(controlSocket->SendMessage(msg->buffer, msg->bufferSize, 0) < 0);
+}
+
+bool TargetEthController::IsControllerActionSupported(
+    ControllerAction action) const {
+  // TODO: here we should ask the remote side (host). Probably through the
+  // control socket
+  switch (action) {
+  case ControllerAction::SetLedOn:
+    return true;
+  case ControllerAction::SetLedOff:
+    return true;
+  case ControllerAction::Rumble:
+    return true;
+  case ControllerAction::FlashLed:
+    return true;
+  default:
+    return false;
+  }
+}
+
+bool TargetEthController::ControllerInitialization() {
+  char message[1024];
+  ssize_t received;
+  bool connected = false;
+  bool mustReadMore;
+
+  listeningSocket->Listen(listeningPort);
+  Thread::Info("Debug: Listening to port %d\n", listeningPort);
+  // accept incoming connection
+  bool connectionAccepted = false;
+  while (!connectionAccepted) {
+    controlSocket = listeningSocket->Accept(10);
+    if (controlSocket == nullptr) {
+      // Timeout (or error btw)
+      if (ToBeStopped())
         return false;
-    }
-}
-
-bool TargetEthController::ControllerInitialization() {
-    char message[1024];
-    ssize_t received;
-    bool connected=false;
-    bool mustReadMore;
-
-    listeningSocket->Listen(listeningPort);
-    Thread::Info("Debug: Listening to port %d\n",listeningPort);
-                 //accept incoming connection
-    bool connectionAccepted=false;
-    while (!connectionAccepted) {
-        controlSocket=listeningSocket->Accept(10);
-        if (controlSocket==nullptr) {
-            //Timeout (or error btw)
-            if (ToBeStopped()) return false;
-        } else connectionAccepted=true;
-    }
-    Thread::Info("Debug: Connexion accepted\n");
-
-    //get axis stuff
-    bool axisNumberRead=false;
-    while (!axisNumberRead) {
-        try {
-            axisNumber=controlSocket->ReadUInt32(10);
-            //Thread::Info("Debug: axisNumber %d\n", axisNumber);
-            axisNumberRead=true;
-        }
-        catch (std::runtime_error e) {
-            //timeout
-            if (ToBeStopped()) return false;
-        }
-    }
-    bool bitsPerAxisRead=false;
-    while (!bitsPerAxisRead) {
-        try {
-            bitsPerAxis=controlSocket->ReadUInt32(10);
-            //Thread::Info("Debug: bits per axis %d\n", bitsPerAxis);
-            bitsPerAxisRead=true;
-        }
-        catch (std::runtime_error e) {
-            //timeout
-            if (ToBeStopped()) return false;
-        }
-    }
-    axisName=new string[axisNumber];
-    for (unsigned int i=0;i<axisNumber;i++) {
-        //read string size
-        int stringSize;
-        bool stringSizeRead=false;
-        while (!stringSizeRead) {
-            try {
-                stringSize=controlSocket->ReadUInt32(10);
-                stringSizeRead=true;
-            }
-            catch (std::runtime_error e) {
-                //timeout
-                if (ToBeStopped()) return false;
-            }
-        }
-        //read string
-        bool axisNameRead=false;
-        while (!axisNameRead) {
-            try {
-                axisName[i]=controlSocket->ReadString(stringSize,10);
-                axisNameRead=true;
-            }
-            catch (std::runtime_error e) {
-                //timeout
-                if (ToBeStopped()) return false;
-            }
-        }
-        //Thread::Info("Debug: axisName for axis %d %s\n", i, axisName[i].c_str());
-    }
-
-    //get button stuff
-    bool buttonNumberRead=false;
-    while (!buttonNumberRead) {
-        try {
-            buttonNumber=controlSocket->ReadUInt32(10);
-            buttonNumberRead=true;
-        }
-        catch (std::runtime_error e) {
-            //timeout
-            if (ToBeStopped()) return false;
-        }
-    }
-    //Thread::Info("Debug: buttonNumber %d\n", buttonNumber);
-    buttonName=new string[buttonNumber];
-    for (unsigned int i=0;i<buttonNumber;i++) {
-        //read string size
-        int stringSize;
-        bool stringSizeRead=false;
-        while (!stringSizeRead) {
-            try {
-                stringSize=controlSocket->ReadUInt32(10);
-                stringSizeRead=true;
-            }
-            catch (std::runtime_error e) {
-                //timeout
-                if (ToBeStopped()) return false;
-            }
-        }
-        //read string
-        bool buttonNameRead=false;
-        while (!buttonNameRead) {
-            try {
-                buttonName[i]=controlSocket->ReadString(stringSize,10);
-                buttonNameRead=true;
-            }
-            catch (std::runtime_error e) {
-                //timeout
-                if (ToBeStopped()) return false;
-            }
-        }
-        //Thread::Info("Debug: buttonName for button %d %s\n", i, buttonName[i].c_str());
-    }
-
-//    dataFrameSize=axisNumber*sizeof(float)+buttonNumber/8*sizeof(uint8_t);
-    buttonOffset=(axisNumber*bitsPerAxis)/8;
-    if ((axisNumber*bitsPerAxis)%8!=0) buttonOffset++;
-    dataFrameSize=buttonOffset+(buttonNumber/8)*sizeof(uint8_t);
-    if ((buttonNumber%8)!=0) dataFrameSize++;
-    dataFrameBuffer=new char[dataFrameSize];
-    receiveFrameBuffer=new char[dataFrameSize];
-
-    Thread::Info("Controller connected with host side\n");
-    if(buttonNumber%8!=0) Thread::Err("Button number is not multiple of 8\n");
-    return true;
+    } else
+      connectionAccepted = true;
+  }
+  Thread::Info("Debug: Connexion accepted\n");
+
+  // get axis stuff
+  bool axisNumberRead = false;
+  while (!axisNumberRead) {
+    try {
+      axisNumber = controlSocket->ReadUInt32(10);
+      // Thread::Info("Debug: axisNumber %d\n", axisNumber);
+      axisNumberRead = true;
+    } catch (std::runtime_error e) {
+      // timeout
+      if (ToBeStopped())
+        return false;
+    }
+  }
+  bool bitsPerAxisRead = false;
+  while (!bitsPerAxisRead) {
+    try {
+      bitsPerAxis = controlSocket->ReadUInt32(10);
+      // Thread::Info("Debug: bits per axis %d\n", bitsPerAxis);
+      bitsPerAxisRead = true;
+    } catch (std::runtime_error e) {
+      // timeout
+      if (ToBeStopped())
+        return false;
+    }
+  }
+  axisName = new string[axisNumber];
+  for (unsigned int i = 0; i < axisNumber; i++) {
+    // read string size
+    int stringSize;
+    bool stringSizeRead = false;
+    while (!stringSizeRead) {
+      try {
+        stringSize = controlSocket->ReadUInt32(10);
+        stringSizeRead = true;
+      } catch (std::runtime_error e) {
+        // timeout
+        if (ToBeStopped())
+          return false;
+      }
+    }
+    // read string
+    bool axisNameRead = false;
+    while (!axisNameRead) {
+      try {
+        axisName[i] = controlSocket->ReadString(stringSize, 10);
+        axisNameRead = true;
+      } catch (std::runtime_error e) {
+        // timeout
+        if (ToBeStopped())
+          return false;
+      }
+    }
+    // Thread::Info("Debug: axisName for axis %d %s\n", i, axisName[i].c_str());
+  }
+
+  // get button stuff
+  bool buttonNumberRead = false;
+  while (!buttonNumberRead) {
+    try {
+      buttonNumber = controlSocket->ReadUInt32(10);
+      buttonNumberRead = true;
+    } catch (std::runtime_error e) {
+      // timeout
+      if (ToBeStopped())
+        return false;
+    }
+  }
+  // Thread::Info("Debug: buttonNumber %d\n", buttonNumber);
+  buttonName = new string[buttonNumber];
+  for (unsigned int i = 0; i < buttonNumber; i++) {
+    // read string size
+    int stringSize;
+    bool stringSizeRead = false;
+    while (!stringSizeRead) {
+      try {
+        stringSize = controlSocket->ReadUInt32(10);
+        stringSizeRead = true;
+      } catch (std::runtime_error e) {
+        // timeout
+        if (ToBeStopped())
+          return false;
+      }
+    }
+    // read string
+    bool buttonNameRead = false;
+    while (!buttonNameRead) {
+      try {
+        buttonName[i] = controlSocket->ReadString(stringSize, 10);
+        buttonNameRead = true;
+      } catch (std::runtime_error e) {
+        // timeout
+        if (ToBeStopped())
+          return false;
+      }
+    }
+    // Thread::Info("Debug: buttonName for button %d %s\n", i,
+    // buttonName[i].c_str());
+  }
+
+  //    dataFrameSize=axisNumber*sizeof(float)+buttonNumber/8*sizeof(uint8_t);
+  buttonOffset = (axisNumber * bitsPerAxis) / 8;
+  if ((axisNumber * bitsPerAxis) % 8 != 0)
+    buttonOffset++;
+  dataFrameSize = buttonOffset + (buttonNumber / 8) * sizeof(uint8_t);
+  if ((buttonNumber % 8) != 0)
+    dataFrameSize++;
+  dataFrameBuffer = new char[dataFrameSize];
+  receiveFrameBuffer = new char[dataFrameSize];
+
+  Thread::Info("Controller connected with host side\n");
+  if (buttonNumber % 8 != 0)
+    Thread::Err("Button number is not multiple of 8\n");
+  return true;
 }