source: flair-src/trunk/lib/FlairIpc/src/IpcController.cpp@ 444

Last change on this file since 444 was 397, checked in by Sanahuja Guillaume, 4 years ago

add ipc lib
  • Property svn:eol-style set to native
File size: 11.6 KB
RevLine 
[397]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: 2021/03/03
6// filename: IpcController.cpp
7//
8// author: Sébastien Ambroziak
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 "IpcController.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
36IpcController::IpcController(string name, uint16_t _port,
37 uint8_t priority)
38 : TargetController(name, priority), listeningPort(_port),
39 receiveCurrentPosition(0) {
40
41 Thread::Err("ipc is not yet implemented!\n");
42
43 const bool blocking = true;
44 listeningSocket =
45 new TcpSocket(getFrameworkManager(), "TEC_listening_socket", blocking, blocking);
46 dataSocket =
47 new UdpSocket(getFrameworkManager(), "TEC_data_socket", _port + 1); // receiving side
48}
49
50IpcController::~IpcController() {
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 delete controlSocket;
65 }
66
67 // TargetController calls IpcController methods in its run
68 // we must stop the thread now
69 SafeStop();
70 Join();
71 delete dataSocket;
72 delete listeningSocket;
73}
74
75bool IpcController::IsConnected() const {
76 // TODO
77}
78
79bool IpcController::IsDataFrameReady() {
80 // read up to the last data
81 ssize_t received;
82 size_t bytesToReceive = dataFrameSize - receiveCurrentPosition;
83 bool fullDatagramReceived = false;
84
85 do {
86 received=dataSocket->RecvMessage(receiveFrameBuffer+receiveCurrentPosition,bytesToReceive,TIME_NONBLOCK);
87 if (received>0) {
88 receiveCurrentPosition+=received;
89 bytesToReceive-=received;
90 if (bytesToReceive==0) {
91 //a full datagram has been read in receiveFrameBuffer
92 fullDatagramReceived=true;
93 //we swap the data and reception buffers to avoid copy
94 char *swapFrameBuffer=dataFrameBuffer;
95 dataFrameBuffer=receiveFrameBuffer;
96 receiveFrameBuffer=swapFrameBuffer;
97 receiveCurrentPosition=0;
98 bytesToReceive=dataFrameSize;
99 }
100 }
101 } while (!(received<0));
102
103 return fullDatagramReceived;
104}
105
106uint8_t IpcController::getByteOrNull(char *buffer, int byte,
107 size_t bufferSize) {
108 if (byte < bufferSize)
109 return buffer[byte];
110 else
111 return 0;
112}
113
114uint32_t IpcController::charBufferToUint32(char *buffer,
115 size_t bufferSize) {
116 union {
117 uint32_t int32;
118 char byte[4];
119 } bitField;
120 if (!IsBigEndian()) {
121 bitField.byte[0] = getByteOrNull(buffer, 3, bufferSize);
122 bitField.byte[1] = getByteOrNull(buffer, 2, bufferSize);
123 bitField.byte[2] = getByteOrNull(buffer, 1, bufferSize);
124 bitField.byte[3] = getByteOrNull(buffer, 0, bufferSize);
125 } else {
126 bitField.byte[0] = getByteOrNull(buffer, 0, bufferSize);
127 bitField.byte[1] = getByteOrNull(buffer, 1, bufferSize);
128 bitField.byte[2] = getByteOrNull(buffer, 2, bufferSize);
129 bitField.byte[3] = getByteOrNull(buffer, 3, bufferSize);
130 }
131 return bitField.int32;
132}
133
134// read _up_to_16_bits_ in a buffer
135uint16_t IpcController::readBits(uint8_t offsetInBits,
136 uint8_t valueSizeInBits, char *buffer,
137 size_t bufferSize) {
138 // parameters check
139 if (valueSizeInBits > 16)
140 throw std::range_error("bitfield should be at max 16bits wide");
141 size_t minBufferSize = (offsetInBits + valueSizeInBits) / 8;
142 if ((offsetInBits + valueSizeInBits) % 8 != 0)
143 minBufferSize++;
144 if (bufferSize < minBufferSize)
145 throw std::range_error("buffer too small");
146 // skip first bytes
147 size_t bytesToSkip = offsetInBits / 8;
148 buffer += bytesToSkip;
149 bufferSize -= bytesToSkip;
150 offsetInBits -= bytesToSkip * 8;
151 // take care of endianness
152 uint32_t value = charBufferToUint32(buffer, bufferSize);
153 value >>= 32 - offsetInBits - valueSizeInBits;
154 value &= (1 << valueSizeInBits) - 1;
155 return (uint16_t)value;
156}
157
158void IpcController::AcquireAxisData(core::Matrix &axis) {
159 axis.GetMutex();
160 // char testFrameBuffer[3]={(char)0x09,(char)0x59,(char)0xB8};
161 for (unsigned int i = 0; i < axisNumber; i++) {
162 uint16_t rawAxisValue =
163 readBits(i * bitsPerAxis, bitsPerAxis, dataFrameBuffer, dataFrameSize);
164 // uint16_t
165 // rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,testFrameBuffer);
166 uint16_t scale = 1 << (bitsPerAxis - 1);
167 // Thread::Info("RawAxisValue=%d, scale=%d => scaled rawValue=%d, float
168 // value)%f\n",rawAxisValue,scale,rawAxisValue-scale,(rawAxisValue-scale)/(float)scale);
169 axis.SetValueNoMutex(i, 0, (rawAxisValue - scale) / (float)scale);
170 }
171 axis.ReleaseMutex();
172}
173
174void IpcController::AcquireButtonData(core::Matrix &button) {
175 uint8_t buttonValue;
176 int currentButton = 0;
177 button.GetMutex();
178 /*
179 for (unsigned int i=0;i<buttonNumber;i++) {
180 memcpy(&buttonValue,buttonOffset+i*sizeof(bool),sizeof(bool));
181 dataSocket->NetworkToHost((char*)&buttonValue,sizeof(bool));
182 button.SetValueNoMutex(i,0,buttonValue);
183// if (buttonValue) Thread::Info("Debug: button '%s' pressed\n",
184GetButtonName(i).c_str());
185 }*/
186 for (unsigned int i = 0; i < buttonNumber / 8; i++) {
187 memcpy(&buttonValue, dataFrameBuffer + buttonOffset + i * sizeof(uint8_t),
188 sizeof(uint8_t));
189 // dataSocket->NetworkToHost((char*)&buttonValue,sizeof(uint8_t));
190 for (unsigned int j = 0; j < 8; j++) {
191 button.SetValueNoMutex(currentButton, 0, (buttonValue >> j) & 0x01);
192 currentButton++;
193 }
194 }
195 button.ReleaseMutex();
196}
197
198string IpcController::GetAxisName(unsigned int axisId) const {
199 // TODO: should throw an exception if axisName==NULL or axisId>axisNumber
200 return axisName[axisId];
201}
202
203string IpcController::GetButtonName(unsigned int buttonId) const {
204 // TODO: should throw an exception if buttonName==NULL or
205 // buttonId>buttonNumber
206 return buttonName[buttonId];
207}
208
209bool IpcController::ProcessMessage(Message *msg) {
210 return !(controlSocket->SendMessage(msg->buffer, msg->bufferSize, TIME_INFINITE) < 0);
211}
212
213bool IpcController::IsControllerActionSupported(
214 ControllerAction action) const {
215 // TODO: here we should ask the remote side (host). Probably through the
216 // control socket
217 switch (action) {
218 case ControllerAction::SetLedOn:
219 return true;
220 case ControllerAction::SetLedOff:
221 return true;
222 case ControllerAction::Rumble:
223 return true;
224 case ControllerAction::FlashLed:
225 return true;
226 default:
227 return false;
228 }
229}
230
231bool IpcController::ControllerInitialization() {
232 char message[1024];
233 ssize_t received;
234 bool connected = false;
235 bool mustReadMore;
236
237 listeningSocket->Listen(listeningPort);
238 Thread::Info("Debug: Listening to port %d\n", listeningPort);
239 // accept incoming connection
240 while (!controlSocket) {
241 try {
242 controlSocket = listeningSocket->Accept(100000000);
243 } catch (std::logic_error &e) {
244 Thread::Err("%s\n",e.what());
245 if (ToBeStopped())
246 return false;
247 } catch (std::runtime_error e) {
248 // timeout
249 //Thread::Err("%s\n",e.what());
250 if (ToBeStopped())
251 return false;
252 }
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.