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

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

correction sémaphore. bloquant tout ça...

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  bool connectionAccepted = false;
238  while (!connectionAccepted) {
239    try {
240      controlSocket = listeningSocket->Accept(100000000);
241    } catch (std::logic_error &e) {
242      Thread::Err(e.what());
243      return false;
244    } catch (std::runtime_error e) {
245      // timeout
246      if (ToBeStopped())
247        return false;
248    }
249    if (controlSocket == nullptr) {
250      return false;
251    } else
252      connectionAccepted = true;
253  }
254  Thread::Info("Debug: Connexion accepted\n");
255
256  // get axis stuff
257  bool axisNumberRead = false;
258  while (!axisNumberRead) {
259    try {
260      axisNumber = controlSocket->ReadUInt32(100000000);
261      // Thread::Info("Debug: axisNumber %d\n", axisNumber);
262      axisNumberRead = true;
263    } catch (std::runtime_error e) {
264      // timeout
265      if (ToBeStopped())
266        return false;
267    }
268  }
269  bool bitsPerAxisRead = false;
270  while (!bitsPerAxisRead) {
271    try {
272      bitsPerAxis = controlSocket->ReadUInt32(100000000);
273      // Thread::Info("Debug: bits per axis %d\n", bitsPerAxis);
274      bitsPerAxisRead = true;
275    } catch (std::runtime_error e) {
276      // timeout
277      if (ToBeStopped())
278        return false;
279    }
280  }
281  axisName = new string[axisNumber];
282  for (unsigned int i = 0; i < axisNumber; i++) {
283    // read string size
284    int stringSize;
285    bool stringSizeRead = false;
286    while (!stringSizeRead) {
287      try {
288        stringSize = controlSocket->ReadUInt32(100000000);
289        stringSizeRead = true;
290      } catch (std::runtime_error e) {
291        // timeout
292        if (ToBeStopped())
293          return false;
294      }
295    }
296    // read string
297    bool axisNameRead = false;
298    while (!axisNameRead) {
299      try {
300        axisName[i] = controlSocket->ReadString(stringSize, 100000000);
301        axisNameRead = true;
302      } catch (std::runtime_error e) {
303        // timeout
304        if (ToBeStopped())
305          return false;
306      }
307    }
308    // Thread::Info("Debug: axisName for axis %d %s\n", i, axisName[i].c_str());
309  }
310
311  // get button stuff
312  bool buttonNumberRead = false;
313  while (!buttonNumberRead) {
314    try {
315      buttonNumber = controlSocket->ReadUInt32(100000000);
316      buttonNumberRead = true;
317    } catch (std::runtime_error e) {
318      // timeout
319      if (ToBeStopped())
320        return false;
321    }
322  }
323  // Thread::Info("Debug: buttonNumber %d\n", buttonNumber);
324  buttonName = new string[buttonNumber];
325  for (unsigned int i = 0; i < buttonNumber; i++) {
326    // read string size
327    int stringSize;
328    bool stringSizeRead = false;
329    while (!stringSizeRead) {
330      try {
331        stringSize = controlSocket->ReadUInt32(100000000);
332        stringSizeRead = true;
333      } catch (std::runtime_error e) {
334        // timeout
335        if (ToBeStopped())
336          return false;
337      }
338    }
339    // read string
340    bool buttonNameRead = false;
341    while (!buttonNameRead) {
342      try {
343        buttonName[i] = controlSocket->ReadString(stringSize, 10000000);
344        buttonNameRead = true;
345      } catch (std::runtime_error e) {
346        // timeout
347        if (ToBeStopped())
348          return false;
349      }
350    }
351    // Thread::Info("Debug: buttonName for button %d %s\n", i,
352    // buttonName[i].c_str());
353  }
354
355  //    dataFrameSize=axisNumber*sizeof(float)+buttonNumber/8*sizeof(uint8_t);
356  buttonOffset = (axisNumber * bitsPerAxis) / 8;
357  if ((axisNumber * bitsPerAxis) % 8 != 0)
358    buttonOffset++;
359  dataFrameSize = buttonOffset + (buttonNumber / 8) * sizeof(uint8_t);
360  if ((buttonNumber % 8) != 0)
361    dataFrameSize++;
362  dataFrameBuffer = new char[dataFrameSize];
363  receiveFrameBuffer = new char[dataFrameSize];
364
365  Thread::Info("Controller connected with host side\n");
366  if (buttonNumber % 8 != 0)
367    Thread::Err("Button number is not multiple of 8\n");
368  return true;
369}
370
371} // end namespace sensor
372} // end namespace flair
Note: See TracBrowser for help on using the repository browser.