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