source: flair-src/branches/sanscv/lib/FlairSensorActuator/src/HostEthController.cpp@ 422

Last change on this file since 422 was 324, checked in by Sanahuja Guillaume, 5 years ago

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