iCub-main
EsdMessageSniffer.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4  * Copyright (C) 2006 Giorgio Metta
5  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
6  *
7  */
8 
9 //
10 // $Id: EsdMessageSniffer.cpp,v 1.3 2008/07/25 15:20:13 iron Exp $
11 //
12 
13 // general purpose stuff.
14 #include <yarp/os/Time.h>
15 #include <ace/config.h>
16 #include <ace/OS.h>
17 #include <ace/Log_Msg.h>
18 #include <ace/Sched_Params.h>
19 
20 // specific to this device driver.
21 #include "EsdMessageSniffer.h"
22 // changed path here; if this doesn't work for you, please fix
23 // libYARP_dev/CMakeLists.txt
24 
25 #include "ntcan.h"
26 #ifdef WIN32
27 #ifndef NTCAN_HANDLE
28 #define NTCAN_HANDLE HANDLE
29 #endif
30 #endif
31 
32 #include <yarp/dev/ControlBoardInterfacesImpl.h>
33 
34 // get the message types from the DSP code.
35 #include "messages.h"
36 
37 #define BUF_SIZE 2047
38 
39 typedef int (*PV) (const char *fmt, ...);
40 
41 using namespace yarp::os;
42 using namespace yarp::dev;
43 
47 const int ESD_MAX_CARDS = 16;
48 
49 EsdMessageSnifferParameters::EsdMessageSnifferParameters(int nj)
50 {
51  _destinations = allocAndCheck<unsigned char> (ESD_MAX_CARDS);
52  _axisMap = allocAndCheck<int>(nj);
53  _angleToEncoder = allocAndCheck<double>(nj);
54  _zeros = allocAndCheck<double>(nj);
55 
56  _networkN = 0;
57  _my_address = 0;
58  _polling_interval = 10;
59  _timeout = 20;
60  _njoints = 0;
61  _p = NULL;
62 
63  _txQueueSize = 2047;
64  _rxQueueSize = 2047;
65  _txTimeout = 20;
66  _rxTimeout = 20;
67 }
68 
69 EsdMessageSnifferParameters::~EsdMessageSnifferParameters()
70 {
71  checkAndDestroy<double>(_zeros);
72  checkAndDestroy<double>(_angleToEncoder);
73  checkAndDestroy<int>(_axisMap);
74  checkAndDestroy<unsigned char>(_destinations);
75 }
76 
77 /*
78  * The brodacast-type message container class.
79  */
80 class BCastBufferElement
81 {
82 public:
83  // msg 1
84  int _position;
85  double _update_p;
86 
87  // msg 2
88  short _pid_value;
89  double _update_v;
90 
91  // msg 3
92  short _fault;
93  double _update_e;
94 
95  // msg 4
96  short _current;
97  short _position_error;
98  double _update_c;
99 
100  BCastBufferElement () { zero (); }
101 
102  void zero (void)
103  {
104  _position = 0;
105  _pid_value = 0;
106  _current = 0;
107  _fault = 0;
108  _position_error = 0;
109 
110  _update_p = .0;
111  _update_v = .0;
112  _update_e = .0;
113  _update_c = .0;
114  }
115 };
116 
117 /*
118  * A container class to deal with the CAN bus API.
119  */
120 class EsdResources
121 {
122 public:
123  EsdResources ();
124  ~EsdResources ();
125 
126  bool initialize (const EsdMessageSnifferParameters& parms);
127  bool uninitialize (void);
128  bool read (void);
129  bool error (const CMSG& m);
130  bool startPacket (void);
131  bool addMessage (int msg_id, int joint);
132  bool writePacket (void);
133  bool printMessage (const CMSG& m);
134  bool dumpBuffers (void);
135  inline int getJoints (void) const { return _njoints; }
136  inline bool getErrorStatus (void) const { return _error_status; }
137 
138 public:
139  enum { ESD_TIMEOUT = 20, ESD_POLLING_INTERVAL = 10 };
140 
141  HANDLE _handle;
142  int _timeout;
143 
144  int _polling_interval;
145  int _speed;
146  int _networkN;
147 
148  long int _txQueueSize;
149  long int _rxQueueSize;
150  long int _txTimeout;
151  long int _rxTimeout;
152 
153  int _readMessages;
154  int _msg_lost;
155  CMSG _readBuffer[BUF_SIZE];
156  int _writeMessages;
157  CMSG _writeBuffer[BUF_SIZE];
158  CMSG _replyBuffer[BUF_SIZE];
159 
160  BCastBufferElement *_bcastRecvBuffer;
161 
162  unsigned char _my_address;
163  unsigned char _destinations[ESD_MAX_CARDS];
164  int _njoints;
165 
166  bool _error_status;
167 
168  PV _p;
170 
171  char _printBuffer[16384];
172 };
173 
174 EsdResources::EsdResources ()
175 {
176  _handle = ACE_INVALID_HANDLE;
177  _timeout = ESD_TIMEOUT;
178  _polling_interval = ESD_POLLING_INTERVAL;
179  _speed = 0;
180  _networkN = 0;
181 
182  ACE_OS::memset(_readBuffer, 0, sizeof(CMSG)*BUF_SIZE);
183  ACE_OS::memset(_writeBuffer, 0, sizeof(CMSG)*BUF_SIZE);
184  ACE_OS::memset(_replyBuffer, 0, sizeof(CMSG)*BUF_SIZE);
185  ACE_OS::memset(_destinations, 0, sizeof(unsigned char) * ESD_MAX_CARDS);
186 
187  _my_address = 0;
188  _njoints = 0;
189  _p = NULL;
190 
191  _readMessages = 0;
192  _msg_lost = 0;
193  _writeMessages = 0;
194  _bcastRecvBuffer = NULL;
195 
196  _error_status = false;
197 }
198 
199 EsdResources::~EsdResources ()
200 {
201  uninitialize();
202 }
203 
204 bool EsdResources::initialize (const EsdMessageSnifferParameters& parms)
205 {
206  if (_handle != ACE_INVALID_HANDLE)
207  return false;
208 
210  _handle = ACE_INVALID_HANDLE;
211  _speed = 0;
212  _networkN = parms._networkN;
213 
214  _readMessages = 0;
215  _writeMessages = 0;
216  _msg_lost = 0;
217  _error_status = false;
218 
219  ACE_OS::memcpy (_destinations, parms._destinations, sizeof(unsigned char)*ESD_MAX_CARDS);
220  _my_address = parms._my_address;
221  _polling_interval = parms._polling_interval;
222  _timeout = parms._timeout;
223  _njoints = parms._njoints;
224  _p = parms._p;
225 
226  _txQueueSize = parms._txQueueSize;
227  _rxQueueSize = parms._rxQueueSize;
228  _txTimeout = parms._txTimeout;
229  _rxTimeout = parms._rxTimeout;
230 
231  _bcastRecvBuffer = new BCastBufferElement[_njoints];
232  ACE_ASSERT (_bcastRecvBuffer != 0);
233 
235  memset (_readBuffer, 0, sizeof(CMSG)*BUF_SIZE);
236  memset (_writeBuffer, 0, sizeof(CMSG)*BUF_SIZE);
237  memset (_replyBuffer, 0, sizeof(CMSG)*BUF_SIZE);
238 
240  int res = canOpen (_networkN, 0, _txQueueSize, _rxQueueSize, _txTimeout, _rxTimeout, &_handle);
241  if (res != NTCAN_SUCCESS)
242  return false;
243 
244  res = canSetBaudrate (_handle, _speed);
245  if (res != NTCAN_SUCCESS)
246  {
247  canClose (_handle);
248  return false;
249  }
250 
252  int i;
253  for (i = 0; i < 0xff; i++)
254  canIdAdd (_handle, i);
255 
256  for (i = 0x100; i < 0x1ff; i++)
257  canIdAdd (_handle, i);
258 
259  return true;
260 }
261 
262 
263 bool EsdResources::uninitialize ()
264 {
265  if (_bcastRecvBuffer != NULL)
266  {
267  delete[] _bcastRecvBuffer;
268  _bcastRecvBuffer = NULL;
269  }
270 
271  if (_handle != ACE_INVALID_HANDLE)
272  {
273  int res = canClose (_handle);
274  if (res != NTCAN_SUCCESS)
275  return false;
276  _handle = ACE_INVALID_HANDLE;
277  }
278 
279  return true;
280 }
281 
282 
283 bool EsdResources::read (void)
284 {
285 #ifdef WIN32
286  long messages=BUF_SIZE;
287 #else
288  int32_t messages = BUF_SIZE;
289 #endif
290 
291  int res = canTake (_handle, _readBuffer, &messages);
292  if (res != NTCAN_SUCCESS)
293  return false;
294 
295  _readMessages = messages;
296  return true;
297 }
298 
299 bool EsdResources::startPacket (void)
300 {
301  _writeMessages = 0;
302  return true;
303 }
304 
305 bool EsdResources::addMessage (int msg_id, int joint)
306 {
307  CMSG x;
308  memset (&x, 0, sizeof(CMSG));
309 
310  x.id = _my_address << 4;
311  x.id = _destinations[joint/2] & 0x0f;
312 
313  x.len = 1;
314  x.data[0] = msg_id;
315  if ((joint % 2) == 1)
316  x.data[0] |= 0x80;
317 
318  _writeBuffer[_writeMessages] = x;
319  _writeMessages ++;
320 
321  return true;
322 }
323 
324 bool EsdResources::writePacket ()
325 {
326  if (_writeMessages < 1)
327  return false;
328 
329 #ifdef WIN32
330  long len=_writeMessages;
331 #else
332  int32_t len =_writeMessages;
333 #endif
334 
335  int res = canSend (_handle, _writeBuffer, &len);
336  if (res != NTCAN_SUCCESS || len != _writeMessages)
337  return false;
338 
339  return true;
340 }
341 
342 bool EsdResources::printMessage (const CMSG& m)
343 {
344  if (!_p)
345  return false;
346 
347  int ret = ACE_OS::sprintf (_printBuffer, "class: %2d s: %2x d: %2x c: %1d msg: %3d (%x) ",
348  (m.id & 0x700) >> 8,
349  (m.id & 0xf0) >> 4,
350  (m.id & 0x0f),
351  ((m.data[0] & 0x80)==0)?0:1,
352  (m.data[0] & 0x7f),
353  (m.data[0] & 0x7f));
354 
355  if (m.len > 1)
356  {
357  ret += ACE_OS::sprintf (_printBuffer+ret, "x: ");
358  }
359 
360  int j;
361  for (j = 1; j < m.len; j++)
362  {
363  ret += ACE_OS::sprintf (_printBuffer+ret, "%x ", m.data[j]);
364  }
365  ret += ACE_OS::sprintf(_printBuffer+ret, "st: %x\n", m.msg_lost);
366 
367  (*_p)
368  ("%s", _printBuffer);
369 
370  return true;
371 }
372 
373 
374 bool EsdResources::dumpBuffers (void)
375 {
376  if (!_p) return false;
377 
378  int j;
379 
381  (*_p) ("Sniffer: write buffer\n");
382  for (j = 0; j < _writeMessages; j++)
383  printMessage (_writeBuffer[j]);
384 
385  (*_p) ("Sniffer: reply buffer\n");
386  for (j = 0; j < _writeMessages; j++)
387  printMessage (_replyBuffer[j]);
388 
389  (*_p) ("Sniffer: read buffer\n");
390  for (j = 0; j < _readMessages; j++)
391  printMessage (_readBuffer[j]);
392  (*_p) ("Sniffer: -------------\n");
393 
394  return true;
395 }
396 
397 bool EsdResources::error (const CMSG& m)
398 {
399  if (m.len & NTCAN_NO_DATA) return true;
400  if (m.msg_lost != 0) { _msg_lost = m.msg_lost; return true; }
401 
402  return false;
403 }
404 
408 inline EsdResources& RES(void *res) { return *(EsdResources *)res; }
409 
410 
411 EsdMessageSniffer::EsdMessageSniffer(void) :
412  ImplementPidControl<EsdMessageSniffer, IPidControl>(this),
413  ImplementAmplifierControl<EsdMessageSniffer, IAmplifierControl>(this),
414  ImplementEncoders<EsdMessageSniffer, IEncoders>(this)
415 {
416  system_resources = (void *) new EsdResources;
417  ACE_ASSERT (system_resources != NULL);
418 }
419 
420 
421 EsdMessageSniffer::~EsdMessageSniffer ()
422 {
423  if (system_resources != NULL)
424  delete (EsdResources *)system_resources;
425  system_resources = NULL;
426 }
427 
428 bool EsdMessageSniffer::open(yarp::os::Searchable& config)
429 {
430  Property p;
431  p.fromString(config.toString());
432 
433  if (!p.check("GENERAL")) {
434  ACE_OS::fprintf(stderr, "Cannot understand configuration parameters\n");
435  return false;
436  }
437 
438  int i;
439  int nj = p.findGroup("GENERAL").find("Joints").asInt32();
440  EsdMessageSnifferParameters params(nj);
441  params._njoints = nj;
442 
444  Bottle &xtmp = p.findGroup("CAN").findGroup("CanDeviceNum");
445  params._networkN=xtmp.get(1).asInt32();
446 
447  xtmp = p.findGroup("CAN").findGroup("CanMyAddress");
448  params._my_address=xtmp.get(1).asInt32();
449 
450  xtmp = p.findGroup("CAN").findGroup("CanPollingInterval");
451  params._polling_interval=xtmp.get(1).asInt32();
452 
453  xtmp = p.findGroup("CAN").findGroup("CanTimeout");
454  params._timeout=xtmp.get(1).asInt32();
455 
456  xtmp = p.findGroup("CAN").findGroup("CanAddresses");
457  for (i = 1; i < xtmp.size(); i++) params._destinations[i-1] = (unsigned char)(xtmp.get(i).asInt32());
458 
460  xtmp = p.findGroup("GENERAL").findGroup("AxisMap");
461  ACE_ASSERT (xtmp.size() == nj+1);
462  for (i = 1; i < xtmp.size(); i++) params._axisMap[i-1] = xtmp.get(i).asInt32();
463  xtmp = p.findGroup("GENERAL").findGroup("Encoder");
464  ACE_ASSERT (xtmp.size() == nj+1);
465  for (i = 1; i < xtmp.size(); i++) params._angleToEncoder[i-1] = xtmp.get(i).asFloat64();
466  xtmp = p.findGroup("GENERAL").findGroup("Zeros");
467  ACE_ASSERT (xtmp.size() == nj+1);
468  for (i = 1; i < xtmp.size(); i++) params._zeros[i-1] = xtmp.get(i).asFloat64();
469 
470  if (p.findGroup("GENERAL").find("Verbose").asInt32() == 1)
471  params._p = ACE_OS::printf;
472  else
473  params._p = NULL;
474 
475  return open(params);
476 }
477 
478 bool EsdMessageSniffer::open (EsdMessageSnifferParameters& parms)
479 {
480  std::lock_guard<std::mutex> lck(_mutex);
481  EsdResources& r = RES (system_resources);
482  if (!r.initialize (parms))
483  {
484  return false;
485  }
486 
487  // initialize the implementation.
488  ImplementPidControl<EsdMessageSniffer, IPidControl>::
489  initialize(parms._njoints, parms._axisMap, parms._angleToEncoder, parms._zeros);
490  ImplementAmplifierControl<EsdMessageSniffer, IAmplifierControl>::
491  initialize(parms._njoints, parms._axisMap, parms._angleToEncoder, parms._zeros);
492  ImplementEncoders<EsdMessageSniffer, IEncoders>::
493  initialize(parms._njoints, parms._axisMap, parms._angleToEncoder, parms._zeros);
494 
495  start();
496 
497  std::unique_lock<std::mutex> lck(mtx_done);
498  cv_done.wait(lck);
499 
500  // used for printing debug messages.
501  _p = parms._p;
502  _writerequested = false;
503  _noreply = false;
504 
505  return true;
506 }
507 
508 bool EsdMessageSniffer::close (void)
509 {
510  ACE_OS::fprintf(stderr, "EsdSniffer::close called\n");
511  EsdResources& d = RES(system_resources);
512  Thread::stop ();
513  ImplementPidControl<EsdMessageSniffer, IPidControl>::uninitialize();
514  ImplementAmplifierControl<EsdMessageSniffer, IAmplifierControl>::uninitialize();
515  ImplementEncoders<EsdMessageSniffer, IEncoders>::uninitialize();
516  return d.uninitialize ();
517 }
518 
519 //
520 //
521 //
522 void EsdMessageSniffer::run(void)
523 {
524  EsdResources& r = RES (system_resources);
525 
527  bool messagePending = false;
528  int i = 0;
529  int remainingMsgs = 0;
530  bool noreply = false;
531  double now = 0;
532  double before = 0;
533  int counter = 0;
534 
535  _writerequested = false;
536  _noreply = false;
537 
538  r._error_status = false;
539 
541  cv_done.notify_one();
542 
543  while (!isStopping() || messagePending)
544  {
545  before = Time::now();
546 
547  _mutex.lock ();
548  if (r.read () != true)
549  if (r._p)
550  (*r._p) ("Sniffer: read failed\n");
551 
552  // handle broadcast messages.
553  // (class 1, 8 bits of the ID used to define the message type and source address).
554  //
555  for (i = 0; i < r._readMessages; i++)
556  {
557  CMSG& m = r._readBuffer[i];
558  if (m.len & NTCAN_NO_DATA)
559  if (r._p)
560  {
561  (*r._p) ("Sniffer: error in message %x len: %d type: %x: %x\n",
562  m.id, m.len, m.data[0], m.msg_lost);
563  continue;
564  }
565 
566  if ((m.id & 0x700) == 0x100) // class = 1.
567  {
568  // 4 next bits = source address, next 4 bits = msg type
569  // this allows sending two 32-bit numbers is a single CAN message.
570  //
571  // need an array here for storing the messages on a per-joint basis.
572 
573  const int addr = ((m.id & 0x0f0) >> 4);
574  int j;
575  for (j = 0; j < ESD_MAX_CARDS; j++)
576  {
577  if (r._destinations[j] == addr)
578  break;
579  }
580 
581  j *= 2;
582 
583  /* less sign nibble specifies msg type */
584  switch (m.id & 0x00f)
585  {
586  case CAN_BCAST_POSITION:
587  r._bcastRecvBuffer[j]._position = *((int *)(m.data));
588  r._bcastRecvBuffer[j]._update_p = before;
589  j++;
590  if (j < r.getJoints())
591  {
592  r._bcastRecvBuffer[j]._position = *((int *)(m.data+4));
593  r._bcastRecvBuffer[j]._update_p = before;
594  }
595  break;
596 
597  case CAN_BCAST_PID_VAL:
598  r._bcastRecvBuffer[j]._pid_value = *((short *)(m.data));
599  r._bcastRecvBuffer[j]._update_v = before;
600 
601  j++;
602  if (j < r.getJoints())
603  {
604  r._bcastRecvBuffer[j]._pid_value = *((short *)(m.data+2));
605  r._bcastRecvBuffer[j]._update_v = before;
606  }
607  break;
608 
609  case CAN_BCAST_STATUS:
610  // fault signals.
611  r._bcastRecvBuffer[j]._fault = *((short *)(m.data));
612  r._bcastRecvBuffer[j]._update_e = before;
613  j++;
614 
615  if (j < r.getJoints())
616  {
617  r._bcastRecvBuffer[j]._fault = *((short *)(m.data+2));
618  r._bcastRecvBuffer[j]._update_e = before;
619  }
620  break;
621 
622  case CAN_BCAST_CURRENT:
623  // also receives the control values.
624  r._bcastRecvBuffer[j]._current = *((short *)(m.data));
625  r._bcastRecvBuffer[j]._position_error = *((short *)(m.data+4));
626  r._bcastRecvBuffer[j]._update_c = before;
627  j++;
628  if (j < r.getJoints())
629  {
630  r._bcastRecvBuffer[j]._current = *((short *)(m.data+2));
631  r._bcastRecvBuffer[j]._position_error = *((short *)(m.data+6));
632  r._bcastRecvBuffer[j]._update_c = before;
633  }
634  break;
635 
636  default:
637  break;
638  }
639  }
640  }
641 
642  //
643  // handle class 0 messages - polling messages.
644  // (class 0, 8 bits of the ID used to represent the source and destination).
645  // the first byte of the message is the message type and motor number (0 or 1).
646  //
647  if (messagePending)
648  {
649  for (i = 0; i < r._readMessages; i++)
650  {
651  CMSG& m = r._readBuffer[i];
652  if (m.len & NTCAN_NO_DATA)
653  if (r._p)
654  {
655  (*r._p) ("Sniffer: error in message %x len: %d type: %x: %x\n",
656  m.id, m.len, m.data[0], m.msg_lost);
657 
658  continue;
659  }
660 
661  if (((m.id &0x700) == 0) &&
662  ((m.data[0] & 0x7f) < NUM_OF_MESSAGES))
663  r.printMessage (m);
664 
665  if (!noreply)
666  {
667  if (((m.id & 0x700) == 0) &&
668  ((m.id & 0x0f) == r._my_address))
669  {
671  int j;
672  for (j = 0; j < r._writeMessages; j++)
673  {
674  if (((r._writeBuffer[j].id & 0x0f) == ((m.id & 0xf0) >> 4)) &&
675  (m.data[0] == r._writeBuffer[j].data[0]))
676  {
677  if (r._replyBuffer[j].id != 0)
678  {
679  if (r._p)
680  {
681  (*r._p) ("Sniffer: message %x was already replied\n", m.id);
682  r.printMessage (m);
683  }
684  }
685  else
686  {
687  ACE_OS::memcpy (&r._replyBuffer[j], &m, sizeof(CMSG));
688  remainingMsgs --;
689  if (remainingMsgs < 1)
690  {
691  messagePending = false;
692  r._error_status = false;
693  goto AckMessageLoop;
694  }
695  }
696  }
697  }
698  }
699  }
700  }
701 
703  if (noreply)
704  {
705  remainingMsgs = 0;
706  messagePending = false;
707  r._error_status = false;
708  goto AckMessageLoop;
709  }
710 
712  counter ++;
713  if (counter > r._timeout)
714  {
716  if (r._p)
717  {
718  (*r._p) ("Sniffer: timeout - still %d messages unacknowledged\n", remainingMsgs);
719  r.dumpBuffers ();
720  }
721 
722  messagePending = false;
723  r._error_status = true;
724  goto AckMessageLoop;
725  }
726 
727  AckMessageLoop:
728  if (!messagePending)
729  {
731  cv_done.notify_one();
732  }
733  }
734  else
735  {
737  if (_writerequested)
738  {
739  if (r._writeMessages > 0)
740  {
741  if (r.writePacket () != true)
742  {
743  if (r._p)
744  {
745  (*r._p) ("Sniffer: write message of %d elments failed\n", r._writeMessages);
746  }
747  }
748  else
749  {
750  messagePending = true;
751  _writerequested = false;
752  remainingMsgs = r._writeMessages;
753  noreply = _noreply;
754  r._error_status = false;
755  counter = 0;
756  memset (r._replyBuffer, 0, sizeof(CMSG) * r._writeMessages);
757 
758  if (r._p)
759  {
760  int j;
761  for (j = 0; j < r._writeMessages; j++)
762  {
763  r.printMessage (r._writeBuffer[j]);
764  }
765  }
766  }
767  }
768  }
769  }
770 
771  _mutex.unlock ();
772 
774  now = Time::now();
775  if ((now - before)*1000 < r._polling_interval)
776  {
777  double k = double(r._polling_interval)/1000.0-(now-before);
778  Time::delay(k);
779  //before = now + k;
780  }
781  else
782  {
783  if (r._p) (*r._p)("Sniffer: thread can't poll fast enough (time: %f)\n", now-before);
784  //before = now;
785  }
786  }
787 
789 }
790 
791 
792 bool EsdMessageSniffer::setDebugPrintFunction (void *cmd)
793 {
794  std::lock_guard<std::mutex> lck(_mutex);
795  // EsdResources& r = RES(system_resources);
796  _p = (int (*) (const char *fmt, ...))cmd;
797  return true;
798 }
799 
800 
805 inline bool EsdMessageSniffer::ENABLED (int axis)
806 {
807  EsdResources& r = RES(system_resources);
808  return ((r._destinations[axis/2] & ESD_CAN_SKIP_ADDR) == 0) ? true : false;
809 }
810 
811 // return the number of controlled axes.
812 bool EsdMessageSniffer::getAxes(int *ax)
813 {
814  EsdResources& r = RES(system_resources);
815  *ax = r.getJoints();
816  return true;
817 }
818 
824 bool EsdMessageSniffer::setBCastMessages (int i, int value)
825 {
826  ACE_ASSERT (i >= 0 && i <= (ESD_MAX_CARDS-1)*2);
827  return _writeDWord (CAN_SET_BCAST_POLICY, i, S_32(value));
828 }
829 
834 bool EsdMessageSniffer::getBCastPosition (int i, double *value)
835 {
836  EsdResources& r = RES(system_resources);
837  ACE_ASSERT (i >= 0 && i <= r.getJoints());
838 
839  std::lock_guard<std::mutex> lck(_mutex);
840  *value = double(r._bcastRecvBuffer[i]._position);
841  return true;
842 }
843 
844 bool EsdMessageSniffer::getBCastPositions (double *p)
845 {
846  EsdResources& r = RES(system_resources);
847  int i;
848  std::lock_guard<std::mutex> lck(_mutex);
849  for (i = 0; i < r.getJoints(); i++)
850  {
851  p[i] = double(r._bcastRecvBuffer[i]._position);
852  }
853  return true;
854 }
855 
856 bool EsdMessageSniffer::getBCastPIDOutput (int i, double *value)
857 {
858  EsdResources& r = RES(system_resources);
859  ACE_ASSERT (i >= 0 && i <= r.getJoints());
860 
861  lock_guard<mutex> lck(_mutex);
862  *value = double(r._bcastRecvBuffer[i]._pid_value);
863  return true;
864 }
865 
866 bool EsdMessageSniffer::getBCastPIDOutputs (double *p)
867 {
868  EsdResources& r = RES(system_resources);
869  int i;
870  std::lock_guard<std::mutex> lck(_mutex);
871  for (i = 0; i < r.getJoints(); i++)
872  {
873  p[i] = double(r._bcastRecvBuffer[i]._pid_value);
874  }
875  return true;
876 }
877 
878 bool EsdMessageSniffer::getBCastCurrent (int i, double *value)
879 {
880  EsdResources& r = RES(system_resources);
881  ACE_ASSERT (i >= 0 && i <= r.getJoints());
882 
883  std::lock_guard<std::mutex> lck(_mutex);
884  *value = double(r._bcastRecvBuffer[i]._current);
885  return true;
886 }
887 
888 bool EsdMessageSniffer::getBCastCurrents (double *p)
889 {
890  EsdResources& r = RES(system_resources);
891  int i;
892 
893  std::lock_guard<std::mutex> lck(_mutex);
894  for (i = 0; i < r.getJoints(); i++)
895  {
896  p[i] = double(r._bcastRecvBuffer[i]._current);
897  }
898  return true;
899 }
900 
901 bool EsdMessageSniffer::getBCastFaults (int *p)
902 {
903  EsdResources& r = RES(system_resources);
904  int i;
905 
906  std::lock_guard<std::mutex> lck(_mutex);
907  for (i = 0; i < r.getJoints(); i++)
908  {
909  p[i] = short(r._bcastRecvBuffer[i]._fault);
910  }
911  return true;
912 }
913 
914 bool EsdMessageSniffer::getBCastPositionError (int i, double *value)
915 {
916  EsdResources& r = RES(system_resources);
917  ACE_ASSERT (i >= 0 && i <= r.getJoints());
918 
919  std::lock_guard<std::mutex> lck(_mutex);
920  *value = double(r._bcastRecvBuffer[i]._position_error);
921  return true;
922 }
923 
924 
925 bool EsdMessageSniffer::getBCastPositionErrors (double *p)
926 {
927  EsdResources& r = RES(system_resources);
928  int i;
929 
930  std::lock_guard<std::mutex> lck(_mutex);
931  for (i = 0; i < r.getJoints(); i++)
932  {
933  p[i] = double(r._bcastRecvBuffer[i]._position_error);
934  }
935  return true;
936 }
937 
938 
944 
946 bool EsdMessageSniffer::_writeNone (int msg, int axis)
947 {
948  EsdResources& r = RES(system_resources);
949  ACE_ASSERT (axis >= 0 && axis <= (ESD_MAX_CARDS-1)*2);
950 
951  if (!ENABLED(axis))
952  {
953  return true;
954  }
955 
956  _mutex.lock();
957 
958  r.startPacket();
959  r.addMessage (msg, axis);
960 
961  _writerequested = true;
962  _noreply = true;
963 
964  _mutex.unlock();
965 
967  std::unique_lock<std::mutex> lck(mtx_done);
968  cv_done.wait(lck);
969 
970  return true;
971 }
972 
973 bool EsdMessageSniffer::_readWord16 (int msg, int axis, short& value)
974 {
975  EsdResources& r = RES(system_resources);
976  ACE_ASSERT (axis >= 0 && axis <= (ESD_MAX_CARDS-1)*2);
977 
978  if (!ENABLED(axis))
979  {
980  value = short(0);
981  return true;
982  }
983 
984  _mutex.lock();
985 
986  r.startPacket();
987  r.addMessage (msg, axis);
988 
989  _writerequested = true;
990  _noreply = false;
991 
992  _mutex.unlock();
993 
995  std::unique_lock<std::mutex> lck(mtx_done);
996  cv_done.wait(lck);
997 
998  if (r.getErrorStatus() != true)
999  {
1000  value = 0;
1001  return false;
1002  }
1003 
1004  value = *((short *)(r._replyBuffer[0].data+1));
1005  return true;
1006 }
1007 
1009 bool EsdMessageSniffer::_readWord16Array (int msg, double *out)
1010 {
1011  EsdResources& r = RES(system_resources);
1012  int i;
1013 
1014  _mutex.lock();
1015  r.startPacket ();
1016 
1017  for(i = 0; i < r.getJoints(); i++)
1018  {
1019  if (ENABLED(i))
1020  {
1021  r.addMessage (msg, i);
1022  }
1023  else
1024  out[i] = 0;
1025  }
1026 
1027  if (r._writeMessages < 1)
1028  {
1029  _mutex.unlock();
1030  return false;
1031  }
1032 
1033  _writerequested = true;
1034  _noreply = false;
1035  _mutex.unlock();
1036 
1037  std::unique_lock<std::mutex> lck(mtx_done);
1038  cv_done.wait(lck);
1039 
1040  if (r.getErrorStatus() != true)
1041  {
1042  ACE_OS::memset (out, 0, sizeof(double) * r.getJoints());
1043  return false;
1044  }
1045 
1046  int j;
1047  for (i = 0, j = 0; i < r.getJoints(); i++)
1048  {
1049  if (ENABLED(i))
1050  {
1051  CMSG& m = r._replyBuffer[j];
1052  if (m.id == 0xffff)
1053  out[i] = 0;
1054  else
1055  out[i] = *((short *)(m.data+1));
1056  j++;
1057  }
1058  }
1059 
1060  return true;
1061 }
1062 
1064 bool EsdMessageSniffer::_writeWord16 (int msg, int axis, short s)
1065 {
1067  EsdResources& r = RES(system_resources);
1068  ACE_ASSERT (axis >= 0 && axis <= (ESD_MAX_CARDS-1)*2);
1069 
1070  if (!ENABLED(axis))
1071  return true;
1072 
1073  _mutex.lock();
1074 
1075  r.startPacket();
1076  r.addMessage (msg, axis);
1077 
1078  *((short *)(r._writeBuffer[0].data+1)) = s;
1079  r._writeBuffer[0].len = 3;
1080 
1081  _writerequested = true;
1082  _noreply = true;
1083 
1084  _mutex.unlock();
1085 
1087  std::unique_lock<std::mutex> lck(mtx_done);
1088  cv_done.wait(lck);
1089 
1091  return true;
1092 }
1093 
1095 bool EsdMessageSniffer::_writeDWord (int msg, int axis, int value)
1096 {
1097  EsdResources& r = RES(system_resources);
1098  ACE_ASSERT (axis >= 0 && axis <= (ESD_MAX_CARDS-1)*2);
1099 
1100  if (!ENABLED(axis))
1101  return true;
1102 
1103  _mutex.wait();
1104 
1105  r.startPacket();
1106  r.addMessage (msg, axis);
1107 
1108  *((int*)(r._writeBuffer[0].data+1)) = value;
1109  r._writeBuffer[0].len = 5;
1110 
1111  _writerequested = true;
1112  _noreply = true;
1113 
1114  _mutex.post();
1115 
1117  std::unique_lock<std::mutex> lck(mtx_done);
1118  cv_done.wait(lck);
1119 
1120  return true;
1121 }
1122 
1124 bool EsdMessageSniffer::_writeWord16Ex (int msg, int axis, short s1, short s2)
1125 {
1127  EsdResources& r = RES(system_resources);
1128 
1129  ACE_ASSERT (axis >= 0 && axis <= (ESD_MAX_CARDS-1)*2);
1130  ACE_ASSERT ((axis % 2) == 0);
1131 
1132  if (!ENABLED(axis))
1133  return true;
1134 
1135  _mutex.lock();
1136 
1137  r.startPacket();
1138  r.addMessage (msg, axis);
1139 
1140  *((short *)(r._writeBuffer[0].data+1)) = s1;
1141  *((short *)(r._writeBuffer[0].data+3)) = s2;
1142  r._writeBuffer[0].len = 5;
1143 
1144  _writerequested = true;
1145  _noreply = true;
1146 
1147  _mutex.unlock();
1148 
1150  std::unique_lock<std::mutex> lck(mtx_done);
1151  cv_done.wait(lck);
1152 
1154  return true;
1155 }
1156 
1160 bool EsdMessageSniffer::_readDWord (int msg, int axis, int& value)
1161 {
1162  EsdResources& r = RES(system_resources);
1163  ACE_ASSERT (axis >= 0 && axis <= (ESD_MAX_CARDS-1)*2);
1164 
1165  if (!ENABLED(axis))
1166  {
1167  value = 0;
1168  return true;
1169  }
1170 
1171  _mutex.lock();
1172 
1173  r.startPacket();
1174  r.addMessage (msg, axis);
1175 
1176  _writerequested = true;
1177  _noreply = false;
1178 
1179  _mutex.unlock();
1180 
1181  std::unique_lock<std::mutex> lck(mtx_done);
1182  cv_done.wait(lck);
1183 
1184  if (r.getErrorStatus() != true)
1185  {
1186  value = 0;
1187  return false;
1188  }
1189 
1190  value = *((int *)(r._replyBuffer[0].data+1));
1191  return true;
1192 }
1193 
1195 bool EsdMessageSniffer::_readDWordArray (int msg, double *out)
1196 {
1197  EsdResources& r = RES(system_resources);
1198  int i=0;
1199 
1200  _mutex.lock();
1201  r.startPacket();
1202 
1203  for (i = 0; i < r.getJoints(); i++)
1204  {
1205  if (ENABLED(i))
1206  {
1207  r.addMessage (msg, i);
1208  }
1209  else
1210  out[i] = 0;
1211  }
1212 
1213  if (r._writeMessages < 1)
1214  {
1215  _mutex.unlock();
1216  return false;
1217  }
1218 
1219  _writerequested = true;
1220  _noreply = false;
1221  _mutex.unlock();
1222 
1223  std::unique_lock<std::mutex> lck(mtx_done);
1224  cv_done.wait(lck);
1225 
1226  if (r.getErrorStatus() != true)
1227  {
1228  ACE_OS::memset (out, 0, sizeof(double) * r.getJoints());
1229  return false;
1230  }
1231 
1232  int j;
1233  for (i = 0, j = 0; i < r.getJoints(); i++)
1234  {
1235  if (ENABLED(i))
1236  {
1237  CMSG& m = r._replyBuffer[j];
1238  if (m.id == 0xffff)
1239  out[i] = 0;
1240  else
1241  out[i] = *((int *)(m.data+1));
1242  j++;
1243  }
1244  }
1245 
1246  return true;
1247 }
1248 
ESD_MAX_CARDS
const int ESD_MAX_CARDS
Max number of addressable cards in this implementation.
Definition: EsdMessageSniffer.cpp:47
RES
EsdResources & RES(void *res)
Definition: EsdMessageSniffer.cpp:408
PV
int(* PV)(const char *fmt,...)
Definition: EsdMessageSniffer.cpp:39
BUF_SIZE
#define BUF_SIZE
Definition: EsdMessageSniffer.cpp:37
EsdMessageSniffer.h
class for interfacing with the ESD can device driver and reading messages.
EsdResources::startPacket
bool startPacket(void)
Definition: EsdMessageSniffer.cpp:299
EsdResources::_readBuffer
CMSG _readBuffer[BUF_SIZE]
Definition: EsdMessageSniffer.cpp:155
EsdResources::uninitialize
bool uninitialize(void)
Definition: EsdMessageSniffer.cpp:263
EsdResources::_error_status
bool _error_status
number of joints (ncards * 2).
Definition: EsdMessageSniffer.cpp:166
EsdResources::_my_address
unsigned char _my_address
local storage for bcast messages.
Definition: EsdMessageSniffer.cpp:162
EsdResources::_replyBuffer
CMSG _replyBuffer[BUF_SIZE]
write buffer.
Definition: EsdMessageSniffer.cpp:158
EsdResources::_timeout
int _timeout
the actual ddriver handle.
Definition: EsdMessageSniffer.cpp:142
EsdResources::_bcastRecvBuffer
BCastBufferElement * _bcastRecvBuffer
reply buffer.
Definition: EsdMessageSniffer.cpp:160
EsdResources::getErrorStatus
bool getErrorStatus(void) const
Definition: EsdMessageSniffer.cpp:136
EsdResources::_destinations
unsigned char _destinations[ESD_MAX_CARDS]
Definition: EsdMessageSniffer.cpp:163
EsdResources::_polling_interval
int _polling_interval
this is my thread timeout.
Definition: EsdMessageSniffer.cpp:144
EsdResources::_speed
int _speed
thread polling interval.
Definition: EsdMessageSniffer.cpp:145
EsdResources::_p
PV _p
error status of the last packet (true = error).
Definition: EsdMessageSniffer.cpp:168
EsdResources::_writeBuffer
CMSG _writeBuffer[BUF_SIZE]
size of the write packet.
Definition: EsdMessageSniffer.cpp:157
EsdResources::getJoints
int getJoints(void) const
Definition: EsdMessageSniffer.cpp:135
EsdResources::_txQueueSize
long int _txQueueSize
network number.
Definition: EsdMessageSniffer.cpp:148
EsdResources::_njoints
int _njoints
list of connected cards (and their addresses).
Definition: EsdMessageSniffer.cpp:164
EsdResources::_networkN
int _networkN
speed of the bus.
Definition: EsdMessageSniffer.cpp:146
EsdResources::error
bool error(const CMSG &m)
Definition: EsdMessageSniffer.cpp:397
EsdResources::dumpBuffers
bool dumpBuffers(void)
Definition: EsdMessageSniffer.cpp:374
EsdResources::addMessage
bool addMessage(int msg_id, int joint)
Definition: EsdMessageSniffer.cpp:305
EsdResources::initialize
bool initialize(const EsdMessageSnifferParameters &parms)
Definition: EsdMessageSniffer.cpp:204
EsdResources::writePacket
bool writePacket(void)
Definition: EsdMessageSniffer.cpp:324
EsdResources::_msg_lost
int _msg_lost
size of the last read buffer.
Definition: EsdMessageSniffer.cpp:154
EsdResources::_writeMessages
int _writeMessages
read buffer.
Definition: EsdMessageSniffer.cpp:156
EsdResources::printMessage
bool printMessage(const CMSG &m)
Definition: EsdMessageSniffer.cpp:342
yarp::dev::EsdMessageSniffer
The EsdMessageSniffer device driver.
Definition: EsdMessageSniffer.h:86
yarp::dev::EsdMessageSniffer::close
virtual bool close(void)
Close the device driver.
Definition: EsdMessageSniffer.cpp:508
yarp::dev::EsdMessageSniffer::getBCastPositionErrors
bool getBCastPositionErrors(double *cmd)
Definition: EsdMessageSniffer.cpp:925
yarp::dev::EsdMessageSniffer::_p
int(* _p)(const char *fmt,...)
pointer to the function printing the device debug information.
Definition: EsdMessageSniffer.h:264
yarp::dev::EsdMessageSniffer::_readWord16
bool _readWord16(int msg, int axis, short &value)
Definition: EsdMessageSniffer.cpp:973
yarp::dev::EsdMessageSniffer::open
virtual bool open(EsdMessageSnifferParameters &params)
Open the device driver.
Definition: EsdMessageSniffer.cpp:478
yarp::dev::EsdMessageSniffer::cv_done
std::condition_variable cv_done
Definition: EsdMessageSniffer.h:248
yarp::dev::EsdMessageSniffer::~EsdMessageSniffer
virtual ~EsdMessageSniffer()
Destructor.
Definition: EsdMessageSniffer.cpp:421
yarp::dev::EsdMessageSniffer::_readWord16Array
bool _readWord16Array(int msg, double *out)
reads an array.
Definition: EsdMessageSniffer.cpp:1009
yarp::dev::EsdMessageSniffer::_writeDWord
bool _writeDWord(int msg, int axis, int value)
write a DWord
Definition: EsdMessageSniffer.cpp:1095
yarp::dev::EsdMessageSniffer::_writeWord16Ex
bool _writeWord16Ex(int msg, int axis, short s1, short s2)
two shorts in a single Can message (both must belong to the same control card).
Definition: EsdMessageSniffer.cpp:1124
yarp::dev::EsdMessageSniffer::getAxes
virtual bool getAxes(int *axis)
Get the number of controlled axes.
Definition: EsdMessageSniffer.cpp:812
yarp::dev::EsdMessageSniffer::getBCastPIDOutput
bool getBCastPIDOutput(int i, double *value)
Definition: EsdMessageSniffer.cpp:856
yarp::dev::EsdMessageSniffer::getBCastCurrent
bool getBCastCurrent(int i, double *value)
Definition: EsdMessageSniffer.cpp:878
yarp::dev::EsdMessageSniffer::_readDWordArray
bool _readDWordArray(int msg, double *out)
reads an array of double words.
Definition: EsdMessageSniffer.cpp:1195
yarp::dev::EsdMessageSniffer::getBCastPosition
bool getBCastPosition(int i, double *value)
reads an array of double from the broadcast message position buffer.
Definition: EsdMessageSniffer.cpp:834
yarp::dev::EsdMessageSniffer::_writeNone
bool _writeNone(int msg, int axis)
helper functions.
Definition: EsdMessageSniffer.cpp:946
yarp::dev::EsdMessageSniffer::ENABLED
bool ENABLED(int axis)
helper function to check whether the enabled flag is on or off.
Definition: EsdMessageSniffer.cpp:805
yarp::dev::EsdMessageSniffer::_writeWord16
bool _writeWord16(int msg, int axis, short s)
helper functions
Definition: EsdMessageSniffer.cpp:1064
yarp::dev::EsdMessageSniffer::_readDWord
bool _readDWord(int msg, int axis, int &value)
sends a message and gets a dword back.
Definition: EsdMessageSniffer.cpp:1160
yarp::dev::EsdMessageSniffer::setBCastMessages
bool setBCastMessages(int i, int value)
sets the broadcast policy for a given board (don't need to be called twice).
Definition: EsdMessageSniffer.cpp:824
yarp::dev::EsdMessageSniffer::getBCastPositionError
bool getBCastPositionError(int i, double *value)
Definition: EsdMessageSniffer.cpp:914
x
x
Definition: compute_ekf_sym.m:21
cmd
cmd
Definition: dataTypes.h:30
CAN_BCAST_POSITION
#define CAN_BCAST_POSITION
Definition: fakeBoard.cpp:9
eth::parser::read
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
Definition: ethParser.cpp:92
strain::dsp::q15::zero
const Q15 zero
Definition: strain.h:67
fprintf
fprintf(fid,'\n')
p
p
Definition: show_eyes_axes.m:23
out
out
Definition: sine.m:8
yarp::dev::EsdMessageSnifferParameters
A container class to pass parameters to the open() method of the device driver.
Definition: EsdMessageSniffer.h:41
yarp::dev::EsdMessageSnifferParameters::_my_address
unsigned char _my_address
number of joints (cards * 2)
Definition: EsdMessageSniffer.h:60
yarp::dev::EsdMessageSnifferParameters::_timeout
int _timeout
thread polling interval [ms]
Definition: EsdMessageSniffer.h:62
yarp::dev::EsdMessageSnifferParameters::_p
int(* _p)(const char *fmt,...)
number of cycles before timing out
Definition: EsdMessageSniffer.h:63
yarp::dev::EsdMessageSnifferParameters::_destinations
unsigned char * _destinations
printf-like function for spying messages
Definition: EsdMessageSniffer.h:65
yarp::dev::EsdMessageSnifferParameters::_axisMap
int * _axisMap
destination addresses
Definition: EsdMessageSniffer.h:66
yarp::dev::EsdMessageSnifferParameters::_zeros
double * _zeros
angle to encoder conversion factors
Definition: EsdMessageSniffer.h:68
yarp::dev::EsdMessageSnifferParameters::_angleToEncoder
double * _angleToEncoder
axis remapping lookup-table
Definition: EsdMessageSniffer.h:67
Generated on Thu Apr 11 2024 13:51:00 for iCub-main by doxygen 1.9.1