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

Last change on this file since 4 was 3, checked in by Sanahuja Guillaume, 6 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.