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

Last change on this file since 10 was 3, checked in by Sanahuja Guillaume, 8 years ago

sensoractuator
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 and sends remote control
15// data to a distant target (this class) through Wifi//
16//
17/*********************************************************************/
18#include "TargetEthController.h"
19#include <Controller.h>
20#include <FrameworkManager.h>
21#include <TcpSocket.h>
22#include <Socket.h>
23#include <cstring>
24#include <string>
25#include <cvmatrix.h>
26#include <stdexcept>
27
28#include <unistd.h> // for sleep (debug)
29
30using namespace flair::core;
31using namespace flair::gui;
32using std::string;
33
34namespace flair
35{
36namespace sensor
37{
38
39TargetEthController::TargetEthController(const FrameworkManager* parent,string name,uint16_t _port,uint8_t priority) :
40 TargetController(parent,name,priority),listeningPort(_port),receiveCurrentPosition(0) {
41 const bool blocking=true;
42 listeningSocket=new TcpSocket(parent,"TEC_listening_socket",blocking,blocking);
43 dataSocket=new Socket(parent,"TEC_data_socket",_port+1); //receiving side
44}
45
46TargetEthController::~TargetEthController() {
47 //We are (currently) the server side. We must ask the client side to initiate tcp connexion closing to avoid the server socket
48 //to get stuck in TIME_WAIT state
49 Message msg(32);
50 if (controlSocket) {
51 Message cancelAcquisition(sizeof(ControllerAction));
52 ControllerAction exit=ControllerAction::Exit;
53 memcpy(cancelAcquisition.buffer,&exit,sizeof(ControllerAction));
54 controlSocket->SendMessage(cancelAcquisition.buffer,cancelAcquisition.bufferSize);
55 //We don't expect any more data from the client, we're just waiting for the socket to be closed by the client
56 controlSocket->RecvMessage(msg.buffer,msg.bufferSize);
57 }
58
59 //TargetController calls TargetEthController methods in its run
60 //we must stop the thread now
61 SafeStop();
62 Join();
63}
64
65bool TargetEthController::IsConnected() const {
66 //TODO
67}
68
69bool TargetEthController::IsDataFrameReady(){
70 //read up to the last data
71 ssize_t received;
72 size_t bytesToReceive=dataFrameSize-receiveCurrentPosition;
73 bool fullDatagramReceived=false;
74
75 do {
76 received=dataSocket->RecvMessage(receiveFrameBuffer+receiveCurrentPosition,bytesToReceive,TIME_NONBLOCK);
77 if (received>0) {
78 bytesToReceive-=received;
79 if (bytesToReceive==0) {
80 //a full datagram has been read in receiveFrameBuffer
81 fullDatagramReceived=true;
82 receiveCurrentPosition=0;
83 //we swap the data and reception buffers to avoid copy
84 char *swapFrameBuffer=dataFrameBuffer;
85 dataFrameBuffer=receiveFrameBuffer;
86 receiveFrameBuffer=swapFrameBuffer;
87 }
88 }
89 } while (!received<0);
90
91 return fullDatagramReceived;
92}
93
94uint8_t TargetEthController::getByteOrNull(char *buffer,int byte,size_t bufferSize) {
95 if (byte<bufferSize) return buffer[byte];
96 else return 0;
97}
98
99uint32_t TargetEthController::charBufferToUint32(char *buffer, size_t bufferSize) {
100 union {
101 uint32_t int32;
102 char byte[4];
103 } bitField;
104 if (!IsBigEndian()) {
105 bitField.byte[0]=getByteOrNull(buffer,3,bufferSize);
106 bitField.byte[1]=getByteOrNull(buffer,2,bufferSize);
107 bitField.byte[2]=getByteOrNull(buffer,1,bufferSize);
108 bitField.byte[3]=getByteOrNull(buffer,0,bufferSize);
109 } else {
110 bitField.byte[0]=getByteOrNull(buffer,0,bufferSize);
111 bitField.byte[1]=getByteOrNull(buffer,1,bufferSize);
112 bitField.byte[2]=getByteOrNull(buffer,2,bufferSize);
113 bitField.byte[3]=getByteOrNull(buffer,3,bufferSize);
114 }
115 return bitField.int32;
116}
117
118//read _up_to_16_bits_ in a buffer
119uint16_t TargetEthController::readBits(uint8_t offsetInBits,uint8_t valueSizeInBits,char *buffer,size_t bufferSize) {
120 //parameters check
121 if (valueSizeInBits>16) throw std::range_error("bitfield should be at max 16bits wide");
122 size_t minBufferSize=(offsetInBits+valueSizeInBits)/8;
123 if ((offsetInBits+valueSizeInBits)%8!=0) minBufferSize++;
124 if (bufferSize<minBufferSize) throw std::range_error("buffer too small");
125 //skip first bytes
126 size_t bytesToSkip=offsetInBits/8;
127 buffer+=bytesToSkip;
128 bufferSize-=bytesToSkip;
129 offsetInBits-=bytesToSkip*8;
130 //take care of endianness
131 uint32_t value=charBufferToUint32(buffer,bufferSize);
132 value>>=32-offsetInBits-valueSizeInBits;
133 value&=(1<<valueSizeInBits)-1;
134 return (uint16_t)value;
135}
136
137void TargetEthController::AcquireAxisData(core::cvmatrix &axis) {
138 axis.GetMutex();
139//char testFrameBuffer[3]={(char)0x09,(char)0x59,(char)0xB8};
140 for (unsigned int i=0;i<axisNumber;i++) {
141 uint16_t rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,dataFrameBuffer,dataFrameSize);
142// uint16_t rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,testFrameBuffer);
143 uint16_t scale=1<<(bitsPerAxis-1);
144//Thread::Info("RawAxisValue=%d, scale=%d => scaled rawValue=%d, float value)%f\n",rawAxisValue,scale,rawAxisValue-scale,(rawAxisValue-scale)/(float)scale);
145 axis.SetValueNoMutex(i,0,(rawAxisValue-scale)/(float)scale);
146 }
147 axis.ReleaseMutex();
148}
149
150void TargetEthController::AcquireButtonData(core::cvmatrix &button) {
151 uint8_t buttonValue;
152 int currentButton=0;
153 button.GetMutex();
154 /*
155 for (unsigned int i=0;i<buttonNumber;i++) {
156 memcpy(&buttonValue,buttonOffset+i*sizeof(bool),sizeof(bool));
157 dataSocket->NetworkToHost((char*)&buttonValue,sizeof(bool));
158 button.SetValueNoMutex(i,0,buttonValue);
159// if (buttonValue) Thread::Info("Debug: button '%s' pressed\n", GetButtonName(i).c_str());
160 }*/
161 for (unsigned int i=0;i<buttonNumber/8;i++) {
162 memcpy(&buttonValue,dataFrameBuffer+buttonOffset+i*sizeof(uint8_t),sizeof(uint8_t));
163// dataSocket->NetworkToHost((char*)&buttonValue,sizeof(uint8_t));
164 for(unsigned int j=0;j<8;j++) {
165 button.SetValueNoMutex(currentButton,0,(buttonValue>>j)&0x01);
166 currentButton++;
167 }
168 }
169 button.ReleaseMutex();
170}
171
172string TargetEthController::GetAxisName(unsigned int axisId) const{
173 //TODO: should throw an exception if axisName==NULL or axisId>axisNumber
174 return axisName[axisId];
175}
176
177string TargetEthController::GetButtonName(unsigned int buttonId) const{
178 //TODO: should throw an exception if buttonName==NULL or buttonId>buttonNumber
179 return buttonName[buttonId];
180}
181
182bool TargetEthController::ProcessMessage(Message *msg) {
183 return !(controlSocket->SendMessage(msg->buffer,msg->bufferSize,0)<0);
184}
185
186bool TargetEthController::IsControllerActionSupported(ControllerAction action) const {
187 //TODO: here we should ask the remote side (host). Probably through the control socket
188 switch(action) {
189 case ControllerAction::SetLedOn:
190 return true;
191 case ControllerAction::SetLedOff:
192 return true;
193 case ControllerAction::Rumble:
194 return true;
195 case ControllerAction::FlashLed:
196 return true;
197 default:
198 return false;
199 }
200}
201
202bool TargetEthController::ControllerInitialization() {
203 char message[1024];
204 ssize_t received;
205 bool connected=false;
206 bool mustReadMore;
207
208 listeningSocket->Listen(listeningPort);
209 Thread::Info("Debug: Listening to port %d\n",listeningPort);
210 //accept incoming connection
211 bool connectionAccepted=false;
212 while (!connectionAccepted) {
213 controlSocket=listeningSocket->Accept(10);
214 if (controlSocket==nullptr) {
215 //Timeout (or error btw)
216 if (ToBeStopped()) return false;
217 } else connectionAccepted=true;
218 }
219 Thread::Info("Debug: Connexion accepted\n");
220
221 //get axis stuff
222 bool axisNumberRead=false;
223 while (!axisNumberRead) {
224 try {
225 axisNumber=controlSocket->ReadUInt32(10);
226 //Thread::Info("Debug: axisNumber %d\n", axisNumber);
227 axisNumberRead=true;
228 }
229 catch (std::runtime_error e) {
230 //timeout
231 if (ToBeStopped()) return false;
232 }
233 }
234 bool bitsPerAxisRead=false;
235 while (!bitsPerAxisRead) {
236 try {
237 bitsPerAxis=controlSocket->ReadUInt32(10);
238 //Thread::Info("Debug: bits per axis %d\n", bitsPerAxis);
239 bitsPerAxisRead=true;
240 }
241 catch (std::runtime_error e) {
242 //timeout
243 if (ToBeStopped()) return false;
244 }
245 }
246 axisName=new string[axisNumber];
247 for (unsigned int i=0;i<axisNumber;i++) {
248 //read string size
249 int stringSize;
250 bool stringSizeRead=false;
251 while (!stringSizeRead) {
252 try {
253 stringSize=controlSocket->ReadUInt32(10);
254 stringSizeRead=true;
255 }
256 catch (std::runtime_error e) {
257 //timeout
258 if (ToBeStopped()) return false;
259 }
260 }
261 //read string
262 bool axisNameRead=false;
263 while (!axisNameRead) {
264 try {
265 axisName[i]=controlSocket->ReadString(stringSize,10);
266 axisNameRead=true;
267 }
268 catch (std::runtime_error e) {
269 //timeout
270 if (ToBeStopped()) return false;
271 }
272 }
273 //Thread::Info("Debug: axisName for axis %d %s\n", i, axisName[i].c_str());
274 }
275
276 //get button stuff
277 bool buttonNumberRead=false;
278 while (!buttonNumberRead) {
279 try {
280 buttonNumber=controlSocket->ReadUInt32(10);
281 buttonNumberRead=true;
282 }
283 catch (std::runtime_error e) {
284 //timeout
285 if (ToBeStopped()) return false;
286 }
287 }
288 //Thread::Info("Debug: buttonNumber %d\n", buttonNumber);
289 buttonName=new string[buttonNumber];
290 for (unsigned int i=0;i<buttonNumber;i++) {
291 //read string size
292 int stringSize;
293 bool stringSizeRead=false;
294 while (!stringSizeRead) {
295 try {
296 stringSize=controlSocket->ReadUInt32(10);
297 stringSizeRead=true;
298 }
299 catch (std::runtime_error e) {
300 //timeout
301 if (ToBeStopped()) return false;
302 }
303 }
304 //read string
305 bool buttonNameRead=false;
306 while (!buttonNameRead) {
307 try {
308 buttonName[i]=controlSocket->ReadString(stringSize,10);
309 buttonNameRead=true;
310 }
311 catch (std::runtime_error e) {
312 //timeout
313 if (ToBeStopped()) return false;
314 }
315 }
316 //Thread::Info("Debug: buttonName for button %d %s\n", i, buttonName[i].c_str());
317 }
318
319// dataFrameSize=axisNumber*sizeof(float)+buttonNumber/8*sizeof(uint8_t);
320 buttonOffset=(axisNumber*bitsPerAxis)/8;
321 if ((axisNumber*bitsPerAxis)%8!=0) buttonOffset++;
322 dataFrameSize=buttonOffset+(buttonNumber/8)*sizeof(uint8_t);
323 if ((buttonNumber%8)!=0) dataFrameSize++;
324 dataFrameBuffer=new char[dataFrameSize];
325 receiveFrameBuffer=new char[dataFrameSize];
326
327 Thread::Info("Controller connected with host side\n");
328 if(buttonNumber%8!=0) Thread::Err("Button number is not multiple of 8\n");
329 return true;
330}
331
332} // end namespace sensor
333} // end namespace flair
Note: See TracBrowser for help on using the repository browser.