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

Last change on this file since 214 was 214, checked in by Sanahuja Guillaume, 4 years ago

matrix

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