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

Last change on this file since 330 was 249, checked in by Bayard Gildas, 3 years ago

bug #36 solved

File size: 11.7 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    delete controlSocket;
62  }
63
64  // TargetController calls TargetEthController methods in its run
65  // we must stop the thread now
66  SafeStop();
67  Join();
68  delete dataSocket;
69  delete listeningSocket;
70}
71
72bool TargetEthController::IsConnected() const {
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=dataSocket->RecvMessage(receiveFrameBuffer+receiveCurrentPosition,bytesToReceive,TIME_NONBLOCK);
84        if (received>0) {
85            receiveCurrentPosition+=received;
86            bytesToReceive-=received;
87            if (bytesToReceive==0) {
88                //a full datagram has been read in receiveFrameBuffer
89                fullDatagramReceived=true;
90                //we swap the data and reception buffers to avoid copy
91                char *swapFrameBuffer=dataFrameBuffer;
92                dataFrameBuffer=receiveFrameBuffer;
93                receiveFrameBuffer=swapFrameBuffer;
94                receiveCurrentPosition=0;
95                bytesToReceive=dataFrameSize;
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;
153}
154
155void TargetEthController::AcquireAxisData(core::Matrix &axis) {
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();
169}
170
171void TargetEthController::AcquireButtonData(core::Matrix &button) {
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];
204}
205
206bool TargetEthController::ProcessMessage(Message *msg) {
207  return !(controlSocket->SendMessage(msg->buffer, msg->bufferSize, TIME_INFINITE) < 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  while (!controlSocket) {
238    try {
239      controlSocket = listeningSocket->Accept(100000000);
240    } catch (std::logic_error &e) {
241      Thread::Err("%s\n",e.what());
242      if (ToBeStopped())
243        return false;
244    } catch (std::runtime_error e) {
245      // timeout
246      //Thread::Err("%s\n",e.what());
247      if (ToBeStopped())
248        return false;
249    }
250  }
251  Thread::Info("Debug: Connexion accepted\n");
252
253  // get axis stuff
254  bool axisNumberRead = false;
255  while (!axisNumberRead) {
256    try {
257      axisNumber = controlSocket->ReadUInt32(100000000);
258      // Thread::Info("Debug: axisNumber %d\n", axisNumber);
259      axisNumberRead = true;
260    } catch (std::runtime_error e) {
261      // timeout
262      if (ToBeStopped())
263        return false;
264    }
265  }
266  bool bitsPerAxisRead = false;
267  while (!bitsPerAxisRead) {
268    try {
269      bitsPerAxis = controlSocket->ReadUInt32(100000000);
270      // Thread::Info("Debug: bits per axis %d\n", bitsPerAxis);
271      bitsPerAxisRead = true;
272    } catch (std::runtime_error e) {
273      // timeout
274      if (ToBeStopped())
275        return false;
276    }
277  }
278  axisName = new string[axisNumber];
279  for (unsigned int i = 0; i < axisNumber; i++) {
280    // read string size
281    int stringSize;
282    bool stringSizeRead = false;
283    while (!stringSizeRead) {
284      try {
285        stringSize = controlSocket->ReadUInt32(100000000);
286        stringSizeRead = true;
287      } catch (std::runtime_error e) {
288        // timeout
289        if (ToBeStopped())
290          return false;
291      }
292    }
293    // read string
294    bool axisNameRead = false;
295    while (!axisNameRead) {
296      try {
297        axisName[i] = controlSocket->ReadString(stringSize, 100000000);
298        axisNameRead = true;
299      } catch (std::runtime_error e) {
300        // timeout
301        if (ToBeStopped())
302          return false;
303      }
304    }
305    // Thread::Info("Debug: axisName for axis %d %s\n", i, axisName[i].c_str());
306  }
307
308  // get button stuff
309  bool buttonNumberRead = false;
310  while (!buttonNumberRead) {
311    try {
312      buttonNumber = controlSocket->ReadUInt32(100000000);
313      buttonNumberRead = true;
314    } catch (std::runtime_error e) {
315      // timeout
316      if (ToBeStopped())
317        return false;
318    }
319  }
320  // Thread::Info("Debug: buttonNumber %d\n", buttonNumber);
321  buttonName = new string[buttonNumber];
322  for (unsigned int i = 0; i < buttonNumber; i++) {
323    // read string size
324    int stringSize;
325    bool stringSizeRead = false;
326    while (!stringSizeRead) {
327      try {
328        stringSize = controlSocket->ReadUInt32(100000000);
329        stringSizeRead = true;
330      } catch (std::runtime_error e) {
331        // timeout
332        if (ToBeStopped())
333          return false;
334      }
335    }
336    // read string
337    bool buttonNameRead = false;
338    while (!buttonNameRead) {
339      try {
340        buttonName[i] = controlSocket->ReadString(stringSize, 10000000);
341        buttonNameRead = true;
342      } catch (std::runtime_error e) {
343        // timeout
344        if (ToBeStopped())
345          return false;
346      }
347    }
348    // Thread::Info("Debug: buttonName for button %d %s\n", i,
349    // buttonName[i].c_str());
350  }
351
352  //    dataFrameSize=axisNumber*sizeof(float)+buttonNumber/8*sizeof(uint8_t);
353  buttonOffset = (axisNumber * bitsPerAxis) / 8;
354  if ((axisNumber * bitsPerAxis) % 8 != 0)
355    buttonOffset++;
356  dataFrameSize = buttonOffset + (buttonNumber / 8) * sizeof(uint8_t);
357  if ((buttonNumber % 8) != 0)
358    dataFrameSize++;
359  dataFrameBuffer = new char[dataFrameSize];
360  receiveFrameBuffer = new char[dataFrameSize];
361
362  Thread::Info("Controller connected with host side\n");
363  if (buttonNumber % 8 != 0)
364    Thread::Err("Button number is not multiple of 8\n");
365  return true;
366}
367
368} // end namespace sensor
369} // end namespace flair
Note: See TracBrowser for help on using the repository browser.