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

Last change on this file since 204 was 202, checked in by Sanahuja Guillaume, 7 years ago

modifs usleep
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 <cvmatrix.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::cvmatrix &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::cvmatrix &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.