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

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

singleton manager

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 <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
35namespace flair {
36namespace sensor {
37
38TargetEthController::TargetEthController(string name, uint16_t _port,
39                                         uint8_t priority)
40    : TargetController(name, priority), listeningPort(_port),
41      receiveCurrentPosition(0) {
42  const bool blocking = true;
43  listeningSocket =
44      new TcpSocket(getFrameworkManager(), "TEC_listening_socket", blocking, blocking);
45  dataSocket =
46      new Socket(getFrameworkManager(), "TEC_data_socket", _port + 1); // receiving side
47}
48
49TargetEthController::~TargetEthController() {
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  }
64
65  // TargetController calls TargetEthController methods in its run
66  // we must stop the thread now
67  SafeStop();
68  Join();
69}
70
71bool TargetEthController::IsConnected() const {
72  // TODO
73}
74
75bool TargetEthController::IsDataFrameReady() {
76  // read up to the last data
77  ssize_t received;
78  size_t bytesToReceive = dataFrameSize - receiveCurrentPosition;
79  bool fullDatagramReceived = false;
80
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));
98
99  return fullDatagramReceived;
100}
101
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;
108}
109
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;
128}
129
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;
152}
153
154void TargetEthController::AcquireAxisData(core::cvmatrix &axis) {
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();
168}
169
170void TargetEthController::AcquireButtonData(core::cvmatrix &button) {
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++;
189    }
190  }
191  button.ReleaseMutex();
192}
193
194string TargetEthController::GetAxisName(unsigned int axisId) const {
195  // TODO: should throw an exception if axisName==NULL or axisId>axisNumber
196  return axisName[axisId];
197}
198
199string TargetEthController::GetButtonName(unsigned int buttonId) const {
200  // TODO: should throw an exception if buttonName==NULL or
201  // buttonId>buttonNumber
202  return buttonName[buttonId];
203}
204
205bool TargetEthController::ProcessMessage(Message *msg) {
206  return !(controlSocket->SendMessage(msg->buffer, msg->bufferSize, 0) < 0);
207}
208
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  }
225}
226
227bool TargetEthController::ControllerInitialization() {
228  char message[1024];
229  ssize_t received;
230  bool connected = false;
231  bool mustReadMore;
232
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");
247
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;
259    }
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;
271    }
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      }
287    }
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  }
302
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;
313    }
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      }
330    }
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  }
346
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];
356
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;
361}
362
363} // end namespace sensor
364} // end namespace flair
Note: See TracBrowser for help on using the repository browser.