source: flair-src/branches/mavlink/lib/FlairSensorActuator/src/TargetEthController.cpp@ 49

Last change on this file since 49 was 38, checked in by Bayard Gildas, 9 years ago

Modif. pour ajour manette émulée (EmulatedController)
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(const FrameworkManager *parent,
39 string name, uint16_t _port,
40 uint8_t priority)
41 : TargetController(parent, name, priority), listeningPort(_port),
42 receiveCurrentPosition(0) {
43 const bool blocking = true;
44 listeningSocket =
45 new TcpSocket(parent, "TEC_listening_socket", blocking, blocking);
46 dataSocket =
47 new Socket(parent, "TEC_data_socket", _port + 1); // receiving side
48}
49
50TargetEthController::~TargetEthController() {
51 // We are (currently) the server side. We must ask the client side to initiate
52 // tcp connexion closing to avoid the server socket
53 // to get stuck in TIME_WAIT state
54 Message msg(32);
55 if (controlSocket) {
56 Message cancelAcquisition(sizeof(ControllerAction));
57 ControllerAction exit = ControllerAction::Exit;
58 memcpy(cancelAcquisition.buffer, &exit, sizeof(ControllerAction));
59 controlSocket->SendMessage(cancelAcquisition.buffer,
60 cancelAcquisition.bufferSize);
61 // We don't expect any more data from the client, we're just waiting for the
62 // socket to be closed by the client
63 controlSocket->RecvMessage(msg.buffer, msg.bufferSize);
64 }
65
66 // TargetController calls TargetEthController methods in its run
67 // we must stop the thread now
68 SafeStop();
69 Join();
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::cvmatrix &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::cvmatrix &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, 0) < 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 controlSocket = listeningSocket->Accept(10);
240 if (controlSocket == nullptr) {
241 // Timeout (or error btw)
242 if (ToBeStopped())
243 return false;
244 } else
245 connectionAccepted = true;
246 }
247 Thread::Info("Debug: Connexion accepted\n");
248
249 // get axis stuff
250 bool axisNumberRead = false;
251 while (!axisNumberRead) {
252 try {
253 axisNumber = controlSocket->ReadUInt32(10);
254 // Thread::Info("Debug: axisNumber %d\n", axisNumber);
255 axisNumberRead = true;
256 } catch (std::runtime_error e) {
257 // timeout
258 if (ToBeStopped())
259 return false;
260 }
261 }
262 bool bitsPerAxisRead = false;
263 while (!bitsPerAxisRead) {
264 try {
265 bitsPerAxis = controlSocket->ReadUInt32(10);
266 // Thread::Info("Debug: bits per axis %d\n", bitsPerAxis);
267 bitsPerAxisRead = true;
268 } catch (std::runtime_error e) {
269 // timeout
270 if (ToBeStopped())
271 return false;
272 }
273 }
274 axisName = new string[axisNumber];
275 for (unsigned int i = 0; i < axisNumber; i++) {
276 // read string size
277 int stringSize;
278 bool stringSizeRead = false;
279 while (!stringSizeRead) {
280 try {
281 stringSize = controlSocket->ReadUInt32(10);
282 stringSizeRead = true;
283 } catch (std::runtime_error e) {
284 // timeout
285 if (ToBeStopped())
286 return false;
287 }
288 }
289 // read string
290 bool axisNameRead = false;
291 while (!axisNameRead) {
292 try {
293 axisName[i] = controlSocket->ReadString(stringSize, 10);
294 axisNameRead = true;
295 } catch (std::runtime_error e) {
296 // timeout
297 if (ToBeStopped())
298 return false;
299 }
300 }
301 // Thread::Info("Debug: axisName for axis %d %s\n", i, axisName[i].c_str());
302 }
303
304 // get button stuff
305 bool buttonNumberRead = false;
306 while (!buttonNumberRead) {
307 try {
308 buttonNumber = controlSocket->ReadUInt32(10);
309 buttonNumberRead = true;
310 } catch (std::runtime_error e) {
311 // timeout
312 if (ToBeStopped())
313 return false;
314 }
315 }
316 // Thread::Info("Debug: buttonNumber %d\n", buttonNumber);
317 buttonName = new string[buttonNumber];
318 for (unsigned int i = 0; i < buttonNumber; i++) {
319 // read string size
320 int stringSize;
321 bool stringSizeRead = false;
322 while (!stringSizeRead) {
323 try {
324 stringSize = controlSocket->ReadUInt32(10);
325 stringSizeRead = true;
326 } catch (std::runtime_error e) {
327 // timeout
328 if (ToBeStopped())
329 return false;
330 }
331 }
332 // read string
333 bool buttonNameRead = false;
334 while (!buttonNameRead) {
335 try {
336 buttonName[i] = controlSocket->ReadString(stringSize, 10);
337 buttonNameRead = true;
338 } catch (std::runtime_error e) {
339 // timeout
340 if (ToBeStopped())
341 return false;
342 }
343 }
344 // Thread::Info("Debug: buttonName for button %d %s\n", i,
345 // buttonName[i].c_str());
346 }
347
348 // dataFrameSize=axisNumber*sizeof(float)+buttonNumber/8*sizeof(uint8_t);
349 buttonOffset = (axisNumber * bitsPerAxis) / 8;
350 if ((axisNumber * bitsPerAxis) % 8 != 0)
351 buttonOffset++;
352 dataFrameSize = buttonOffset + (buttonNumber / 8) * sizeof(uint8_t);
353 if ((buttonNumber % 8) != 0)
354 dataFrameSize++;
355 dataFrameBuffer = new char[dataFrameSize];
356 receiveFrameBuffer = new char[dataFrameSize];
357
358 Thread::Info("Controller connected with host side\n");
359 if (buttonNumber % 8 != 0)
360 Thread::Err("Button number is not multiple of 8\n");
361 return true;
362}
363
364} // end namespace sensor
365} // end namespace flair
Note: See TracBrowser for help on using the repository browser.