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

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

sources reformatted with flair-format-dir script

File size: 10.6 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>
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
[15]40namespace flair {
41namespace sensor {
[3]42
[15]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);
[3]53
[15]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();
[3]63
[15]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);
[3]73}
74
75HostEthController::~HostEthController() {
[15]76  SafeStop();
77  Join();
[3]78
[15]79  if (!getFrameworkManager()->ConnectionLost())
80    delete tab;
[3]81}
82
83void HostEthController::DrawUserInterface() {
[15]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();
[3]94    }
[15]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
[3]101}
102
103string HostEthController::GetAxisDescription(unsigned int axis) {
[15]104  return string("axis") + std::to_string(axis);
[3]105}
106
107string HostEthController::GetButtonDescription(unsigned int button) {
[15]108  return string("button") + std::to_string(button);
[3]109}
110
111bool HostEthController::ControllerInitialization() {
112
[15]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;
[3]121}
122
123void HostEthController::SendControllerInfo() {
[15]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  }
[3]134
[15]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  }
[3]143}
144
145bool HostEthController::ConnectedWithTarget() {
[15]146  char message[1024];
147  ssize_t sent, received;
148  static bool connectionEstablished = false;
[3]149
[15]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;
[3]159}
160
161void HostEthController::Run() {
[15]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  }
[3]168
[15]169  if (buttonNumber % 8 != 0) {
170    SafeStop();
171    Thread::Err("Button number is not multiple of 8\n");
172  }
[3]173
[15]174  while (!ToBeStopped()) {
175    // Thread::Info("Debug: entering acquisition loop\n");
176    if (getFrameworkManager()->ConnectionLost() == true)
177      SafeStop();
[3]178
[15]179    if (IsDataFrameReady()) { // wait for next data frame
180      meaningfulDataAvailable = true;
181      GetAxisData();
182      GetButtonData();
[3]183
[15]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);
[3]201        }
[15]202      }
203    } else { // try to connect even if host is not sending anything
204      ConnectedWithTarget();
[3]205    }
[15]206    // Thread::Info("Debug: exiting acquisition loop\n");
207  }
[3]208}
209
[15]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;
[3]238}
239
240void HostEthController::BuildDataFrame() {
[15]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);
[3]255    }
[15]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]);
[3]263
[15]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    }
[3]274
[15]275    dataSocket->HostToNetwork((char *)&buttonArray[i], sizeof(uint8_t));
276    memcpy(dataFrameBuffer + buttonOffset + i * sizeof(uint8_t),
277           &buttonArray[i], sizeof(uint8_t));
278  }
[3]279}
280
[15]281HostEthController::DataSender::DataSender(Object *parent,
282                                          HostEthController *_hostEthController,
283                                          string name, uint8_t priority)
284    : Thread(parent, name, priority), hostEthController(_hostEthController) {}
[3]285
286void HostEthController::DataSender::Run() {
[15]287  if (getFrameworkManager()->ErrorOccured() == true) {
288    SafeStop();
289  }
[3]290
[15]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);
[3]299    }
[15]300  }
[3]301}
302
303} // end namespace sensor
304} // end namespace flair
Note: See TracBrowser for help on using the repository browser.