source: flair-src/trunk/lib/FlairSensorActuator/src/TargetEthController.cpp @ 157

Last change on this file since 157 was 137, checked in by Sanahuja Guillaume, 5 years ago

singleton manager

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