source: flair-src/trunk/lib/FlairSensorActuator/src/HostEthController.cpp @ 3

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

sensoractuator

File size: 10.1 KB
RevLine 
[3]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:   HostEthController.cpp
7//
8//  author:     Gildas Bayard
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Base class for host side remote controls that talks to target side through ethernet connection
14//
15//
16/*********************************************************************/
17#include "HostEthController.h"
18#include <Controller.h>
19#include <cvmatrix.h>
20#include <Tab.h>
21#include <TabWidget.h>
22#include <Label.h>
23#include <DataPlot1D.h>
24#include <FrameworkManager.h>
25#include <TargetController.h>
26#include <TcpSocket.h>
27#include <Socket.h>
28#include <cstring>
29#include <string>
30#include <cmath> //for sqrt
31#include <sched.h>
32
33#include <iostream>
34
35using namespace flair::core;
36using namespace flair::gui;
37using std::string;
38
39namespace flair
40{
41namespace sensor
42{
43
44HostEthController::HostEthController(const FrameworkManager* parent,string name,string _address,int _port,uint32_t period,uint32_t _bitsPerAxis,uint8_t priority) :
45    Thread(parent,name,priority),IODevice(parent,name),tab(new Tab(parent->GetTabWidget(),name)),axisNumber(0),buttonNumber(0),targetAddress(_address),targetPort(_port),
46    bitsPerAxis(_bitsPerAxis),dataFrameBuffer(NULL),meaningfulDataAvailable(false) {
47    tabWidget=new TabWidget(tab->NewRow(),name);
48
49    const bool blocking=true;
50    controlSocket=new TcpSocket((Thread*)this,"eth controller control socket", blocking, !blocking);
51    dataSocket=new Socket((Thread*)this,"eth controller data socket",_address+":"+std::to_string(_port+1));
52    dataSender=new DataSender((Thread*)this, this, "eth controller data sender thread");
53    dataSender->SetPeriodMS(period);
54    dataSender->Start();
55
56    //test binary
57    /*
58    uint16_t testValue=0b0111011010; //0x1DA
59    char buffer[3]={(char)0b10101011, (char)0b01100101, (char)0b10110110}; // 0xAB65B6
60    writeBits(testValue,10,buffer,7); // 0b1010101 0 11101101 0 0110110 0xAAED36
61    Thread::Info("Debug: buffer after bits written=%X %X\n",buffer[0],buffer[1]);
62    */
63    connectionEstablishedMutex=new Mutex((Thread*)this);
64}
65
66HostEthController::~HostEthController() {
67    SafeStop();
68    Join();
69
70    if(!getFrameworkManager()->ConnectionLost()) delete tab;
71}
72
73void HostEthController::DrawUserInterface() {
74    Tab* plotTab=new Tab(tabWidget,"Measures");
75    axisPlot=new DataPlot1D *[axisNumber];
76    for (unsigned int i=0; i<axisNumber;i++) {
77        //Start a new row or add up to the current row? We try to keep a 4/3 ratio
78        unsigned int columns=sqrt(4.0*axisNumber/3.0);
79        LayoutPosition *position;
80        if (i%columns==0) {
81            position=plotTab->NewRow();
82        } else {
83            position=plotTab->LastRowLastCol();
84        }
85        axisPlot[i]=new DataPlot1D(position,axis->Name(i,0),-(1<<(nativeBitsPerAxis-1))*1.2,(1<<(nativeBitsPerAxis-1))*1.5);
86        axisPlot[i]->AddCurve(axis->Element(i));
87    }
88    //we don't plot the button state for now
89}
90
91string HostEthController::GetAxisDescription(unsigned int axis) {
92    return string("axis")+std::to_string(axis);
93}
94
95string HostEthController::GetButtonDescription(unsigned int button) {
96    return string("button")+std::to_string(button);
97}
98
99bool HostEthController::ControllerInitialization() {
100
101    buttonOffset=(axisNumber*bitsPerAxis)/8;
102    if ((axisNumber*bitsPerAxis)%8!=0) buttonOffset++;
103    dataFrameSize=buttonOffset+(buttonNumber/8)*sizeof(uint8_t);
104    if ((buttonNumber%8)!=0) dataFrameSize++;
105    dataFrameBuffer=new char[dataFrameSize];
106    return true;
107}
108
109void HostEthController::SendControllerInfo() {
110    //send axis info
111    controlSocket->WriteUInt32((uint32_t)axisNumber,0);
112    controlSocket->WriteUInt32(bitsPerAxis,0);
113    for (unsigned int i=0; i<axisNumber; i++) {
114        //Thread::Info("Debug: sending axis name for axis %d = %s (takes up %d bytes)\n",i,GetAxisDescription(i).c_str(),GetAxisDescription(i).length());
115        int stringLength=GetAxisDescription(i).length();
116        controlSocket->WriteUInt32((uint32_t)stringLength,0);
117        controlSocket->SendMessage(GetAxisDescription(i).c_str(), stringLength,0);
118    }
119
120    //send button info
121    controlSocket->WriteUInt32((uint32_t)buttonNumber,0);
122    for (unsigned int i=0; i<buttonNumber; i++) {
123        int stringLength=GetButtonDescription(i).length();
124        controlSocket->WriteUInt32((uint32_t)stringLength,0);
125        controlSocket->SendMessage(GetButtonDescription(i).c_str(), stringLength,0);
126    }
127}
128
129bool HostEthController::ConnectedWithTarget() {
130    char message[1024];
131    ssize_t sent,received;
132    static bool connectionEstablished=false;
133
134    connectionEstablishedMutex->GetMutex();
135    if (!connectionEstablished && controlSocket->Connect(targetPort,targetAddress,10)) {
136        Thread::Info("Connected to %s:%d\n", targetAddress.c_str(), targetPort);
137        SendControllerInfo();
138        connectionEstablished=true;
139    }
140    connectionEstablishedMutex->ReleaseMutex();
141    return connectionEstablished;
142}
143
144void HostEthController::Run() {
145    static int divider=0;
146    Message *msgControllerAction=new Message(1024);
147    if(getFrameworkManager()->ErrorOccured()||!ControllerInitialization()) {
148        SafeStop();
149        Thread::Err("Une erreur a eu lieu, on ne lance pas la boucle\n");
150    }
151
152    if(buttonNumber%8!=0) {
153        SafeStop();
154        Thread::Err("Button number is not multiple of 8\n");
155    }
156
157    while(!ToBeStopped()) {
158        //Thread::Info("Debug: entering acquisition loop\n");
159        if(getFrameworkManager()->ConnectionLost()==true) SafeStop();
160
161        if (IsDataFrameReady()) {  //wait for next data frame
162            meaningfulDataAvailable=true;
163            GetAxisData();
164            GetButtonData();
165
166            if (ConnectedWithTarget()) {
167                //read for commands from the target (remote control state change requests such as rumble or led on/off)
168                ssize_t bytesReceived=controlSocket->RecvMessage((char*)msgControllerAction->buffer,msgControllerAction->bufferSize);
169                if(bytesReceived>0) {
170                    //if the message is a cnx exit request we manage it here, if not it will be managed by the derived class
171                    ControllerAction action;
172                    memcpy(&action,msgControllerAction->buffer,sizeof(ControllerAction));
173                    if (action==ControllerAction::Exit) {
174                        //Thread::Info("Debug: exit request received from server\n");
175                        SafeStop();
176                    }
177                    ProcessMessage(msgControllerAction);
178                }
179            }
180        } else {//try to connect even if host is not sending anything
181            ConnectedWithTarget();
182        }
183        //Thread::Info("Debug: exiting acquisition loop\n");
184    }
185}
186
187bool HostEthController::writeBits(uint16_t value,uint8_t valueSizeInBits,char *buffer,uint8_t offsetInBits) {
188    if (valueSizeInBits>16) return false;
189    uint8_t remainingBitsToWrite=valueSizeInBits;
190    //skip first bytes
191    buffer+=offsetInBits/8;
192    offsetInBits-=(offsetInBits/8)*8;
193    while (remainingBitsToWrite>0) {
194        uint8_t remainingBitsInByteBeforeWrite=8-offsetInBits;
195        uint8_t bitsToWrite=remainingBitsToWrite<remainingBitsInByteBeforeWrite?remainingBitsToWrite:remainingBitsInByteBeforeWrite;
196        uint8_t remainingBitsInByteAfterWrite=remainingBitsInByteBeforeWrite-bitsToWrite;
197        //write in the current byte
198        uint8_t byteMask=((1<<bitsToWrite)-1)<<remainingBitsInByteAfterWrite;
199        (*buffer)&=~byteMask;
200        uint16_t valueMask=(1<<remainingBitsToWrite)-1;
201        (*buffer)|=((value&valueMask)>>(remainingBitsToWrite-bitsToWrite))<<remainingBitsInByteAfterWrite;
202        //update state
203        remainingBitsToWrite-=bitsToWrite;
204        offsetInBits=(offsetInBits+bitsToWrite)%8;
205        buffer++;
206    }
207    return true;
208}
209
210void HostEthController::BuildDataFrame() {
211    //int16_t testValue[4]={-120,-43,27,98}; //0x 88 d5 1b 62
212    for (unsigned int i=0;i<axisNumber;i++) {
213        //We shift value to always be positive (so that division/multiplication by power of 2 can easily be done with bit shifts)
214        uint16_t shiftedNativeAxisValue=axis->Value(i,0)+(1<<(nativeBitsPerAxis-1));
215        //int16_t nativeAxisValue=testValue[i];
216        uint16_t scaledAxisValue;
217        if (bitsPerAxis>nativeBitsPerAxis) {
218            scaledAxisValue=shiftedNativeAxisValue<<(bitsPerAxis-nativeBitsPerAxis);
219        } else {
220            scaledAxisValue=shiftedNativeAxisValue>>(nativeBitsPerAxis-bitsPerAxis);
221        }
222        //Thread::Info("Debug: shiftedNativeAxisValue=%#x, scaled axis value=%#x\n",shiftedNativeAxisValue,scaledAxisValue);
223        unsigned int offsetInBits=i*bitsPerAxis;
224        writeBits(scaledAxisValue,bitsPerAxis,dataFrameBuffer,offsetInBits);
225    }
226    //Thread::Info("Buffer après: %x %x %x\n", dataFrameBuffer[0], dataFrameBuffer[1], dataFrameBuffer[2]);
227
228    int currentButton=0;
229    uint8_t buttonArray[buttonNumber/8];
230    for (unsigned int i=0;i<buttonNumber/8;i++) {
231        buttonArray[i]=0;
232        for(unsigned int j=0;j<8;j++) {
233            bool buttonValue=button->Value(currentButton,0);
234            if(buttonValue) buttonArray[i]+=1<<j;
235            currentButton++;
236        }
237
238        dataSocket->HostToNetwork((char *)&buttonArray[i],sizeof(uint8_t));
239        memcpy(dataFrameBuffer+buttonOffset+i*sizeof(uint8_t),&buttonArray[i],sizeof(uint8_t));
240    }
241}
242
243HostEthController::DataSender::DataSender(Object *parent,HostEthController* _hostEthController,string name,uint8_t priority):Thread(parent,name,priority),hostEthController(_hostEthController) {
244
245}
246
247void HostEthController::DataSender::Run() {
248    if(getFrameworkManager()->ErrorOccured()==true) {
249        SafeStop();
250    }
251
252    while(!ToBeStopped()) {
253        WaitPeriod();
254        if (hostEthController->meaningfulDataAvailable && hostEthController->ConnectedWithTarget()) {
255            //send the data
256            hostEthController->BuildDataFrame();
257            hostEthController->dataSocket->SendMessage(hostEthController->dataFrameBuffer,hostEthController->dataFrameSize);
258        }
259    }
260}
261
262} // end namespace sensor
263} // end namespace flair
Note: See TracBrowser for help on using the repository browser.