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

Last change on this file since 241 was 238, checked in by Bayard Gildas, 7 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.