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

Last change on this file since 196 was 178, checked in by Bayard Gildas, 7 years ago

Change Udp socket object name (from "Socket" to "UdpSocket")
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
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 UdpSocket(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.