iCub-main
Loading...
Searching...
No Matches
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
39typedef int (*PV) (const char *fmt, ...);
40
41using namespace yarp::os;
42using namespace yarp::dev;
43
47const int ESD_MAX_CARDS = 16;
48
49EsdMessageSnifferParameters::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
69EsdMessageSnifferParameters::~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 */
80class BCastBufferElement
81{
82public:
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 */
120class EsdResources
121{
122public:
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
138public:
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
174EsdResources::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
199EsdResources::~EsdResources ()
200{
201 uninitialize();
202}
203
204bool 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
263bool 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
283bool 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
299bool EsdResources::startPacket (void)
300{
301 _writeMessages = 0;
302 return true;
303}
304
305bool 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
324bool 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
342bool 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
374bool 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
397bool 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
408inline EsdResources& RES(void *res) { return *(EsdResources *)res; }
409
410
411EsdMessageSniffer::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
427
428bool 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
478bool 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
508bool 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//
522void 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
792bool 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
805inline 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.
812bool EsdMessageSniffer::getAxes(int *ax)
813{
814 EsdResources& r = RES(system_resources);
815 *ax = r.getJoints();
816 return true;
817}
818
824bool 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
834bool 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
844bool 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
856bool 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
866bool 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
878bool 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
888bool 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
901bool 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
914bool 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
925bool 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
946bool 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
973bool 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
1009bool 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
1064bool 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
1095bool 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
1124bool 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
1160bool 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
1195bool 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
RES
CanBusResources & RES(void *res)
Definition CanBusMotionControl.cpp:442
ESD_MAX_CARDS
const int ESD_MAX_CARDS
Max number of addressable cards in this implementation.
Definition EsdMessageSniffer.cpp:47
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::_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::addMessage
bool addMessage(int msg_id, int joint)
Definition EsdMessageSniffer.cpp:305
EsdResources::initialize
bool initialize(const EsdMessageSnifferParameters &parms)
Definition EsdMessageSniffer.cpp:204
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::_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
error
bool error
Definition debugFunctions.cpp:57
CAN_BCAST_POSITION
#define CAN_BCAST_POSITION
Definition fakeBoard.cpp:9
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::_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 Wed Dec 18 2024 16:34:47 for iCub-main by doxygen 1.9.8