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

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

resolve some bugs when closing connection with gcs

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