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

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

singleton manager

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