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

Last change on this file since 178 was 178, checked in by Bayard Gildas, 4 years ago

Change Udp socket object name (from "Socket" to "UdpSocket?")

File size: 10.5 KB
RevLine 
[3]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[3]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//
[15]13//  purpose:    Base class for host side remote controls that talks to target
14//  side through ethernet connection
[3]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>
[178]28#include <UdpSocket.h>
[3]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
[15]40namespace flair {
41namespace sensor {
[3]42
[137]43HostEthController::HostEthController(string name, string _address, int _port,
[15]44                                     uint32_t period, uint32_t _bitsPerAxis,
45                                     uint8_t priority)
[137]46    : Thread(getFrameworkManager(), name, priority), IODevice(getFrameworkManager(), name),
47      tab(new Tab(getFrameworkManager()->GetTabWidget(), name)), axisNumber(0),
[15]48      buttonNumber(0), targetAddress(_address), targetPort(_port),
49      bitsPerAxis(_bitsPerAxis), dataFrameBuffer(NULL),
50      meaningfulDataAvailable(false) {
51  tabWidget = new TabWidget(tab->NewRow(), name);
[3]52
[15]53  const bool blocking = true;
54  controlSocket = new TcpSocket((Thread *)this, "eth controller control socket",
55                                blocking, !blocking);
[178]56  dataSocket = new UdpSocket((Thread *)this, "eth controller data socket",
[15]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();
[3]62
[15]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);
[3]72}
73
74HostEthController::~HostEthController() {
[15]75  SafeStop();
76  Join();
[3]77
[15]78  if (!getFrameworkManager()->ConnectionLost())
79    delete tab;
[3]80}
81
82void HostEthController::DrawUserInterface() {
[15]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();
[3]93    }
[15]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
[3]100}
101
102string HostEthController::GetAxisDescription(unsigned int axis) {
[15]103  return string("axis") + std::to_string(axis);
[3]104}
105
106string HostEthController::GetButtonDescription(unsigned int button) {
[15]107  return string("button") + std::to_string(button);
[3]108}
109
110bool HostEthController::ControllerInitialization() {
111
[15]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;
[3]120}
121
122void HostEthController::SendControllerInfo() {
[15]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  }
[3]133
[15]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  }
[3]142}
143
144bool HostEthController::ConnectedWithTarget() {
[15]145  char message[1024];
146  ssize_t sent, received;
147  static bool connectionEstablished = false;
[3]148
[15]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;
[3]158}
159
160void HostEthController::Run() {
[15]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  }
[3]167
[15]168  if (buttonNumber % 8 != 0) {
169    SafeStop();
170    Thread::Err("Button number is not multiple of 8\n");
171  }
[3]172
[15]173  while (!ToBeStopped()) {
174    // Thread::Info("Debug: entering acquisition loop\n");
175    if (getFrameworkManager()->ConnectionLost() == true)
176      SafeStop();
[3]177
[15]178    if (IsDataFrameReady()) { // wait for next data frame
179      meaningfulDataAvailable = true;
180      GetAxisData();
181      GetButtonData();
[3]182
[15]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);
[3]200        }
[15]201      }
202    } else { // try to connect even if host is not sending anything
203      ConnectedWithTarget();
[3]204    }
[15]205    // Thread::Info("Debug: exiting acquisition loop\n");
206  }
[3]207}
208
[15]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;
[3]237}
238
239void HostEthController::BuildDataFrame() {
[15]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);
[3]254    }
[15]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]);
[3]262
[15]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    }
[3]273
[15]274    dataSocket->HostToNetwork((char *)&buttonArray[i], sizeof(uint8_t));
275    memcpy(dataFrameBuffer + buttonOffset + i * sizeof(uint8_t),
276           &buttonArray[i], sizeof(uint8_t));
277  }
[3]278}
279
[15]280HostEthController::DataSender::DataSender(Object *parent,
281                                          HostEthController *_hostEthController,
282                                          string name, uint8_t priority)
283    : Thread(parent, name, priority), hostEthController(_hostEthController) {}
[3]284
285void HostEthController::DataSender::Run() {
[15]286  if (getFrameworkManager()->ErrorOccured() == true) {
287    SafeStop();
288  }
[3]289
[15]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);
[3]298    }
[15]299  }
[3]300}
301
302} // end namespace sensor
303} // end namespace flair
Note: See TracBrowser for help on using the repository browser.