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

Last change on this file was 336, checked in by Sanahuja Guillaume, 2 years ago

use float

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 = sqrtf(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.