BLEProthesisInterfaceQtWorker.cpp
Go to the documentation of this file.
1 #include "BLEProthesisInterfaceQtWorker.h"
2 
3 #include <QRegularExpression>
4 
5 #define UARTSERVICEUUID "6e400001-b5a3-f393-e0a9-e50e24dcca9e"
6 #define RXUUID "6e400002-b5a3-f393-e0a9-e50e24dcca9e"
7 #define TXUUID "6e400003-b5a3-f393-e0a9-e50e24dcca9e"
8 
9 BLEProthesisInterfaceQtWorker::BLEProthesisInterfaceQtWorker(const QString& mac, BLEProthesisInterface& owner) :
10  _owner{&owner},
11  _mac{mac}
12 {
13  _timer = startTimer(std::chrono::milliseconds{10});
14 }
15 
16 BLEProthesisInterfaceQtWorker::~BLEProthesisInterfaceQtWorker()
17 {
18  cleanup();
19 }
20 
21 void BLEProthesisInterfaceQtWorker::kill()
22 {
23  _killed = true;
24 }
25 
26 void BLEProthesisInterfaceQtWorker::sendCommand(const std::string& cmd)
27 {
28  std::lock_guard guard(_cmdMutex);
29  _cmd += QString::fromStdString(cmd);
30 }
31 
32 void BLEProthesisInterfaceQtWorker::cleanup()
33 {
34  //stop services etc
35  if (_timer != -1)
36  {
37  killTimer(_timer);
38  _timer = -1;
39  }
40  //disconnect
41  if (_control)
42  {
43  _control->disconnectFromDevice();
44  }
45 
46  //delete
47  if (_service)
48  {
49  delete _service;
50  _service = nullptr;
51  }
52  if (_control)
53  {
54  delete _control;
55  _control = nullptr;
56  }
57  if (_deviceDiscoveryAgent)
58  {
59  delete _deviceDiscoveryAgent;
60  _deviceDiscoveryAgent = nullptr;
61  }
62 }
63 
64 void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
65 {
66  if (_killed)
67  {
68  qDebug() << '[' << _mac << ']' << " Stopping NOW!";
69  cleanup();
70  quit();
71  return;
72  }
73  //discovering
74  if (_owner->_state == State::Created)
75  {
76  _deviceDiscoveryAgent = new QBluetoothDeviceDiscoveryAgent;
77  connect(_deviceDiscoveryAgent, SIGNAL(deviceDiscovered(const QBluetoothDeviceInfo&)),
78  this, SLOT(deviceDiscovered(const QBluetoothDeviceInfo&)));
79  connect(_deviceDiscoveryAgent, SIGNAL(error(QBluetoothDeviceDiscoveryAgent::Error)),
80  this, SLOT(deviceDiscoverError(QBluetoothDeviceDiscoveryAgent::Error)));
81  connect(_deviceDiscoveryAgent, SIGNAL(finished()), this, SLOT(deviceDiscoverFinished()));
82  connect(_deviceDiscoveryAgent, SIGNAL(canceled()), this, SLOT(deviceDiscoverFinished()));
83  qDebug() << '[' << _mac << ']' << " State DiscoveringDevices";
84  _owner->_state = State::DiscoveringDevices;
85  _deviceDiscoveryAgent->start();
86  }
87  else if (_owner->_state == State::DiscoveringDevicesDone)
88  {
89  if (!_deviceDiscovered)
90  {
91  qDebug() << '[' << _mac << ']' << " Device discovering failed!";
92  kill();
93  return;
94  }
95  qDebug() << '[' << _mac << ']' << " State Disconnected";
96  _owner->_state = State::Disconnected;
97  }
98  else if (_owner->_state == State::Disconnected)
99  {
100  if (_service)
101  {
102  delete _service;
103  _service = nullptr;
104  }
105  if (_control)
106  {
107  delete _control;
108  _control = nullptr;
109  }
110  _serviceDiscovered = false;
111  _control = new QLowEnergyController(_currentDevice);
112  _control ->setRemoteAddressType(QLowEnergyController::RandomAddress);
113 
114  connect(_control, SIGNAL(serviceDiscovered(QBluetoothUuid)),
115  this, SLOT(serviceDiscovered(QBluetoothUuid)));
116  connect(_control, SIGNAL(discoveryFinished()),
117  this, SLOT(serviceScanDone()));
118  connect(_control, SIGNAL(error(QLowEnergyController::Error)),
119  this, SLOT(controllerError(QLowEnergyController::Error)));
120  connect(_control, SIGNAL(connected()),
121  this, SLOT(deviceConnected()));
122  connect(_control, SIGNAL(disconnected()),
123  this, SLOT(deviceDisconnected()));
124  _control->connectToDevice();
125  qDebug() << '[' << _mac << ']' << " State Connecting";
126  _owner->_state = State::Connecting;
127  }
128  else if (_owner->_state == State::ConnectingDone)
129  {
130  _control->discoverServices();
131  qDebug() << '[' << _mac << ']' << " State DiscoveringServices";
132  _owner->_state = State::DiscoveringServices;
133  }
134  else if (_owner->_state == State::DiscoveringServicesDone)
135  {
136  if (!_serviceDiscovered)
137  {
138  qDebug() << '[' << _mac << ']' << " Service discovering failed!";
139  kill();
140  return;
141  }
142  _service = _control->createServiceObject(QBluetoothUuid(QUuid(UARTSERVICEUUID)));
143 
144  connect(_service, SIGNAL(stateChanged(QLowEnergyService::ServiceState)),
145  this, SLOT(serviceStateChanged(QLowEnergyService::ServiceState)));
146  connect(_service, SIGNAL(characteristicChanged(QLowEnergyCharacteristic, QByteArray)),
147  this, SLOT(readData(QLowEnergyCharacteristic, QByteArray)));
148  connect(_service, SIGNAL(descriptorWritten(QLowEnergyDescriptor, QByteArray)),
149  this, SLOT(receiveDeviceDisconnec(QLowEnergyDescriptor, QByteArray)));
150 
151  _service->discoverDetails();
152 
153  qDebug() << '[' << _mac << ']' << " State ConnectingService";
154  _owner->_state = State::ConnectingService;
155  }
156  else if (_owner->_state == State::Running) //send data
157  {
158  std::lock_guard g(_cmdMutex);
159  if (_service && _cmd.size() != 0)
160  {
161  const QLowEnergyCharacteristic RxChar = _service->characteristic(QBluetoothUuid(QUuid(RXUUID)));
162  QByteArray data;
163  data.append(_cmd);
164  _service->writeCharacteristic(RxChar, data, QLowEnergyService::WriteWithoutResponse);
165  if (_owner->_verboseSend)
166  {
167  qDebug() << '[' << _mac << ']' << " send: " << _cmd;
168  }
169  _cmd.clear();
170  }
171  }
172 }
173 
174 void BLEProthesisInterfaceQtWorker::deviceDiscovered(const QBluetoothDeviceInfo& device)
175 {
176  if (device.address().toString() == _mac)
177  {
178  qDebug() << '[' << _mac << ']' << " Discovered target device " << device.address().toString();
179  _currentDevice = device;
180  _deviceDiscovered = true;
181  qDebug() << '[' << _mac << ']' << " State DiscoveringDevicesDone";
182  _owner->_state = State::DiscoveringDevicesDone;
183  _deviceDiscoveryAgent->stop();
184  }
185  else
186  {
187  qDebug() << '[' << _mac << ']' << " Discovered device " << device.address().toString();
188  }
189 }
190 
191 void BLEProthesisInterfaceQtWorker::deviceDiscoverFinished()
192 {
193  qDebug() << '[' << _mac << ']' << " Discovering of services done.";
194  _owner->_state = State::DiscoveringDevicesDone;
195 }
196 
197 void BLEProthesisInterfaceQtWorker::deviceDiscoverError(QBluetoothDeviceDiscoveryAgent::Error error)
198 {
199  if (error == QBluetoothDeviceDiscoveryAgent::PoweredOffError)
200  {
201  qDebug() << '[' << _mac << ']' << "The Bluetooth adaptor is powered off, power it on before doing discovery.";
202  }
203  else if (error == QBluetoothDeviceDiscoveryAgent::InputOutputError)
204  {
205  qDebug() << '[' << _mac << ']' << "Writing or reading from the device resulted in an error.";
206  }
207  else
208  {
209  qDebug() << '[' << _mac << ']' << "An unknown error has occurred.";
210  }
211  kill();
212 }
213 
214 void BLEProthesisInterfaceQtWorker::serviceDiscovered(const QBluetoothUuid& gatt)
215 {
216  qDebug() << '[' << _mac << ']' << " Discovered service " << gatt.toString();
217  if (gatt == QBluetoothUuid(QUuid(UARTSERVICEUUID)))
218  {
219  qDebug() << '[' << _mac << ']' << "Discovered UART service " << gatt.toString();
220  _serviceDiscovered = true;
221  _owner->_state = State::DiscoveringServicesDone;
222  }
223 }
224 
225 void BLEProthesisInterfaceQtWorker::serviceDiscoverFinished()
226 {
227  qDebug() << '[' << _mac << ']' << " State DiscoveringServicesDone";
228  _owner->_state = State::DiscoveringServicesDone;
229 }
230 
231 void BLEProthesisInterfaceQtWorker::controllerError(QLowEnergyController::Error error)
232 {
233  qDebug() << '[' << _mac << ']' << " Cannot connect to remote device.";
234  qWarning() << '[' << _mac << ']' << " Controller Error:" << error;
235  kill();
236 }
237 
238 void BLEProthesisInterfaceQtWorker::receiveDeviceDisconnec(const QLowEnergyDescriptor& d, const QByteArray& value)
239 {
240  if (d.isValid() && d == _notificationDescTx && value == QByteArray("0000"))
241  {
242  qDebug() << '[' << _mac << ']' << "Device requests disconnect.";
243  //disabled notifications -> assume disconnect intent
244  _control->disconnectFromDevice();
245  if (_service)
246  {
247  delete _service;
248  _service = nullptr;
249  }
250  }
251 }
252 
253 void BLEProthesisInterfaceQtWorker::serviceStateChanged(QLowEnergyService::ServiceState s)
254 {
255  // A descriptoc can only be written if the service is in the ServiceDiscovered state
256  qDebug() << '[' << _mac << ']' << " serviceStateChanged -> " << s;
257  switch (s)
258  {
259  case QLowEnergyService::ServiceDiscovered:
260  {
261 
262  //looking for the TX characteristic
263  const QLowEnergyCharacteristic TxChar = _service->characteristic(QBluetoothUuid(QUuid(TXUUID)));
264  if (!TxChar.isValid())
265  {
266  qDebug() << '[' << _mac << ']' << " Tx characteristic not found";
267  break;
268  }
269 
270  //looking for the RX characteristic
271  const QLowEnergyCharacteristic RxChar = _service->characteristic(QBluetoothUuid(QUuid(RXUUID)));
272  if (!RxChar.isValid())
273  {
274  qDebug() << '[' << _mac << ']' << " Rx characteristic not found";
275  break;
276  }
277 
278  // Bluetooth LE spec Where a characteristic can be notified, a Client Characteristic Configuration descriptor
279  // shall be included in that characteristic as required by the Bluetooth Core Specification
280  // Tx notify is enabled
281  _notificationDescTx = TxChar.descriptor(QBluetoothUuid::ClientCharacteristicConfiguration);
282  if (_notificationDescTx.isValid())
283  {
284  // enable notification
285  _service->writeDescriptor(_notificationDescTx, QByteArray::fromHex("0100"));
286  qDebug() << '[' << _mac << ']' << " State Running";
287  _owner->_state = State::Running;
288  }
289  }
290  break;
291  default:
292  break;
293  }
294 }
295 
296 void BLEProthesisInterfaceQtWorker::deviceConnected()
297 {
298  qDebug() << '[' << _mac << ']' << " State ConnectingDone";
299  _owner->_state = State::ConnectingDone;
300 }
301 
302 void BLEProthesisInterfaceQtWorker::deviceDisconnected()
303 {
304  qDebug() << '[' << _mac << ']' << " State Disconnected";
305  _owner->_state = State::Disconnected;
306 }
307 
308 
309 template<>
310 void BLEProthesisInterfaceQtWorker::consumeData<BLEProthesisInterface::SensorValueProtocol::tpos_tpwm_fpos_fpwm>()
311 {
312  if (!_valueAkk.contains('\n'))
313  {
314  if (_owner->_verboseReceive)
315  {
316  qDebug() << '[' << _mac << ']' << " data does not contain \\n -> no new sensor values\n Buffer:\n" << _valueAkk;
317  }
318  return;
319  }
320  auto listPacks = _valueAkk.split('\n');
321 
322  if (_owner->_verboseReceive)
323  {
324  qDebug() << '[' << _mac << ']' << " parsing " << listPacks.at(listPacks.size() - 2);
325  }
326 
327  auto listVals = listPacks.at(listPacks.size() - 2).split(' ');
328  _owner->_thumbPos = listVals.at(0).toLong();
329  _owner->_thumbPWM = listVals.at(1).toLong();
330  _owner->_fingerPos = listVals.at(2).toLong();
331  _owner->_fingerPWM = listVals.at(3).toLong();
332 
333  _valueAkk = listPacks.back();
334 }
335 
336 template<>
337 void BLEProthesisInterfaceQtWorker::consumeData<BLEProthesisInterface::SensorValueProtocol::mx_pos_pwm>()
338 {
339  if (!_valueAkk.contains('\n'))
340  {
341  if (_owner->_verboseReceive)
342  {
343  qDebug() << '[' << _mac << ']' << " data does not contain \\n -> no new sensor values\n Buffer:\n" << _valueAkk;
344  }
345  return;
346  }
347  auto listPacks = _valueAkk.split('\n');
348 
349  static const QRegularExpression m2(R"(^M2:[ \t]+Pos.:[ \t]+(-?[1-9][0-9]*|0)[ \t]+PWM:[ \t]+(-?[1-9][0-9]*|0)[ \t\n\r]+$)");
350  static const QRegularExpression m3(R"(^M3:[ \t]+Pos.:[ \t]+(-?[1-9][0-9]*|0)[ \t]+PWM:[ \t]+(-?[1-9][0-9]*|0)[ \t\n\r]+$)");
351 
352  for (int i = 0; i < listPacks.size() - 1; ++i)
353  {
354  if (listPacks.at(i).size() == 0)
355  {
356  continue;
357  }
358  if (_owner->_verboseReceive)
359  {
360  qDebug() << '[' << _mac << ']' << " parsing " << listPacks.at(i);
361  }
362  if (const auto matchM2 = m2.match(listPacks.at(i)); matchM2.hasMatch())
363  {
364  _owner->_thumbPos = matchM2.captured(1).toLong();
365  _owner->_thumbPWM = matchM2.captured(2).toLong();
366  if (_owner->_verboseReceive)
367  {
368  qDebug() << '[' << _mac << ']' << " updated M2";
369  }
370  }
371  else if (const auto matchM3 = m3.match(listPacks.at(i)); matchM3.hasMatch())
372  {
373  _owner->_fingerPos = matchM3.captured(1).toLong();
374  _owner->_fingerPWM = matchM3.captured(2).toLong();
375  if (_owner->_verboseReceive)
376  {
377  qDebug() << '[' << _mac << ']' << " updated M3";
378  }
379  }
380  else
381  {
382  qWarning() << "unknown format for data: " << listPacks.at(i).toLocal8Bit() << "\nSkipping";
383  }
384  }
385  _valueAkk = listPacks.back();
386 }
387 
388 void BLEProthesisInterfaceQtWorker::readData(const QLowEnergyCharacteristic& c, const QByteArray& value)
389 {
390  // ignore any other characteristic change
391  if (c.uuid() != QBluetoothUuid(QUuid(TXUUID)))
392  {
393  return;
394  }
395  if (_owner->_verboseReceive)
396  {
397  qDebug() << '[' << _mac << ']' << " received : " << value;
398  }
399 
400  _valueAkk.append(value.data());
401 
402  switch (_owner->_protocol)
403  {
404  case BLEProthesisInterface::SensorValueProtocol::tpos_tpwm_fpos_fpwm:
405  consumeData<BLEProthesisInterface::SensorValueProtocol::tpos_tpwm_fpos_fpwm>();
406  break;
407  case BLEProthesisInterface::SensorValueProtocol::mx_pos_pwm:
408  consumeData<BLEProthesisInterface::SensorValueProtocol::mx_pos_pwm>();
409  break;
410  }
411 }
BLEProthesisInterface::State::Running
@ Running
BLEProthesisInterface::State::Disconnected
@ Disconnected
BLEProthesisInterface::State::DiscoveringServices
@ DiscoveringServices
BLEProthesisInterface::SensorValueProtocol::tpos_tpwm_fpos_fpwm
@ tpos_tpwm_fpos_fpwm
BLEProthesisInterface::State::ConnectingDone
@ ConnectingDone
UARTSERVICEUUID
#define UARTSERVICEUUID
Definition: BLEProthesisInterfaceQtWorker.cpp:5
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
BLEProthesisInterface::State::Connecting
@ Connecting
TXUUID
#define TXUUID
Definition: BLEProthesisInterfaceQtWorker.cpp:7
RXUUID
#define RXUUID
Definition: BLEProthesisInterfaceQtWorker.cpp:6
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
BLEProthesisInterfaceQtWorker.h
BLEProthesisInterfaceQtWorker::timerEvent
void timerEvent(QTimerEvent *event) override
Definition: BLEProthesisInterfaceQtWorker.cpp:64
BLEProthesisInterface::State::Created
@ Created
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
BLEProthesisInterface::State::DiscoveringServicesDone
@ DiscoveringServicesDone
BLEProthesisInterfaceQtWorker::sendCommand
void sendCommand(const std::string &cmd)
Definition: BLEProthesisInterfaceQtWorker.cpp:26
BLEProthesisInterface::SensorValueProtocol::mx_pos_pwm
@ mx_pos_pwm
BLEProthesisInterfaceQtWorker::~BLEProthesisInterfaceQtWorker
~BLEProthesisInterfaceQtWorker()
Definition: BLEProthesisInterfaceQtWorker.cpp:16
BLEProthesisInterfaceQtWorker::BLEProthesisInterfaceQtWorker
BLEProthesisInterfaceQtWorker(const QString &mac, BLEProthesisInterface &owner)
Definition: BLEProthesisInterfaceQtWorker.cpp:9
BLEProthesisInterface::State::ConnectingService
@ ConnectingService
BLEProthesisInterface
Definition: BLEProthesisInterface.h:10
BLEProthesisInterface::State::DiscoveringDevicesDone
@ DiscoveringDevicesDone
BLEProthesisInterfaceQtWorker::kill
void kill()
Definition: BLEProthesisInterfaceQtWorker.cpp:21
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
BLEProthesisInterface::State::DiscoveringDevices
@ DiscoveringDevices