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

Last change on this file since 16 was 15, checked in by Bayard Gildas, 7 years ago

sources reformatted with flair-format-dir script

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