iCub-main
EsdCan.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) 2008 Robotcub Consortium
5 * Author: Lorenzo Natale
6 * CopyPolicy: Released under the terms of the GNU GPL v2.0.
7 *
8 */
12 
13 #include "ntcan.h"
14 #include "EsdCan.h"
15 #include <yarp/dev/CanBusInterface.h>
16 #include <yarp/os/Bottle.h>
17 
18 using namespace yarp::dev;
19 using namespace yarp::os;
20 
21 const int TX_QUEUE_SIZE=2047;
22 const int RX_QUEUE_SIZE=2047;
23 
25 {
26  handle = new NTCAN_HANDLE;
27 }
28 
30 {
31  if (handle!=0)
32  delete handle;
33 }
34 
35 bool EsdCan::canSetBaudRate(unsigned int rate)
36 {
37 // former #if WIN32
38 #if 0
39  DWORD baud=rate;
40 #else
41  uint32_t baud=rate;
42 #endif
43 
44  int res=::canSetBaudrate(*handle, baud);
45 
46  if (res!=NTCAN_SUCCESS)
47  return false;
48 
49  return true;
50 }
51 
52 bool EsdCan::canGetBaudRate(unsigned int *rate)
53 {
54 // former #if WIN32
55 #if 0
56  DWORD baud;
57 #else
58  uint32_t baud;
59 #endif
60 
61  int res=::canGetBaudrate(*handle, &baud);
62  *rate=baud;
63 
64  if (res!=NTCAN_SUCCESS)
65  return false;
66 
67  return true;
68 }
69 
70 bool EsdCan::canIdAdd(unsigned int id)
71 {
72  int res=::canIdAdd(*handle, static_cast<long>(id));
73  if (res!=NTCAN_SUCCESS)
74  return false;
75 
76  return true;
77 }
78 
79 bool EsdCan::canIdDelete(unsigned int id)
80 {
81  int res=::canIdDelete(*handle, static_cast<long>(id));
82  if (res!=NTCAN_SUCCESS)
83  return false;
84 
85  return true;
86 }
87 
88 bool EsdCan::canRead(CanBuffer &msgs,
89  unsigned int size,
90  unsigned int *read,
91  bool wait)
92 {
93  int res;
94 // former #ifdef WIN32
95  #if 0
96  long lRead=size;
97  #else
98  int32_t lRead=size;
99  #endif
100 
101  CMSG *tmp=reinterpret_cast<CMSG *>(msgs[0].getPointer());
102  if (wait)
103  res=::canRead(*handle, tmp, &lRead, 0);
104  else
105  res=::canTake(*handle, tmp, &lRead);
106 
107  *read=lRead;
108 
109  if ( (res==NTCAN_SUCCESS)||(res==NTCAN_RX_TIMEOUT))
110  return true;
111 
112  fprintf(stderr, "Error: canRead returned with code:%.8X\n", res);
113  return false;
114 }
115 
116 bool EsdCan::canWrite(const CanBuffer &msgs,
117  unsigned int size,
118  unsigned int *sent,
119  bool wait)
120 {
121  int res;
122 // former #ifdef WIN32
123  #if 0
124  long lRead=size;
125  #else
126  int32_t lRead=size;
127  #endif
128 
129  CanBuffer &buffer=const_cast<CanBuffer &>(msgs);
130  const CMSG *tmp=reinterpret_cast<const CMSG *>(buffer[0].getPointer());
131 
132  // if (wait)
133  res=::canWrite(*handle, const_cast<CMSG *>(tmp), &lRead, 0);
134  // else
135  // res=::canSend(*handle, const_cast<CMSG *>(tmp), &lRead);
136 
137  *sent=lRead;
138 
139  if (res!=NTCAN_SUCCESS)
140  {
141  fprintf(stderr, "Error: canWrite returned with code:%.8X\n", res);
142  return false;
143  }
144 
145  return true;
146 }
147 
148 bool EsdCan::open(yarp::os::Searchable &par)
149 {
150  int canTxQueue=TX_QUEUE_SIZE;
151  int canRxQueue=RX_QUEUE_SIZE;
152  int netId =-1;
153  int txTimeout=500;
154  int rxTimeout=500;
155 
156  netId=par.check("CanDeviceNum", Value(-1), "numeric identifier of the can device").asInt32();
157  if (netId == -1) netId=par.check("canDeviceNum", Value(-1), "numeric identifier of the can device").asInt32();
158 
159  txTimeout=par.check("CanTxTimeout", Value(500), "timeout on transmission [ms]").asInt32();
160  if (txTimeout == 500) txTimeout=par.check("canTxTimeout", Value(500), "timeout on transmission [ms]").asInt32();
161 
162  rxTimeout=par.check("CanRxTimeout", Value(500), "timeout on receive when calling blocking read [ms]").asInt32() ;
163  if (rxTimeout == 500) rxTimeout=par.check("canRxTimeout", Value(500), "timeout on receive when calling blocking read [ms]").asInt32() ;
164 
165  canTxQueue=par.check("CanTxQueue", Value(TX_QUEUE_SIZE), "length of tx buffer").asInt32();
166  if (canTxQueue == TX_QUEUE_SIZE) canTxQueue=par.check("canTxQueue", Value(TX_QUEUE_SIZE), "length of tx buffer").asInt32();
167 
168  canRxQueue=par.check("CanRxQueue", Value(RX_QUEUE_SIZE), "length of rx buffer").asInt32() ;
169  if (canRxQueue == RX_QUEUE_SIZE) canRxQueue=par.check("canRxQueue", Value(RX_QUEUE_SIZE), "length of rx buffer").asInt32() ;
170 
171 
172  int mode=0;
173  int res = ::canOpen (netId, mode, canTxQueue, canRxQueue, txTimeout, rxTimeout, handle);
174  if (res != NTCAN_SUCCESS)
175  {
176  //fprintf(stderr, "EsdCan::open() returning false\n");
177  return false;
178  }
179 
180  return true;
181 }
182 
184 {
185  int res;
186  if (!handle)
187  return false;
188 
189  res=::canClose (*handle);
190 
191  if (res!=NTCAN_SUCCESS)
192  return false;
193 
194  delete handle;
195  handle=0;
196  return true;
197 }
const int TX_QUEUE_SIZE
Definition: EsdCan.cpp:21
const int RX_QUEUE_SIZE
Definition: EsdCan.cpp:22
virtual bool close()
Definition: EsdCan.cpp:183
virtual bool canGetBaudRate(unsigned int *rate)
Definition: EsdCan.cpp:52
virtual bool canIdDelete(unsigned int id)
Definition: EsdCan.cpp:79
virtual bool canWrite(const CanBuffer &msgs, unsigned int size, unsigned int *sent, bool wait=false)
Definition: EsdCan.cpp:116
virtual bool open(yarp::os::Searchable &par)
Definition: EsdCan.cpp:148
virtual bool canSetBaudRate(unsigned int rate)
Definition: EsdCan.cpp:35
virtual bool canIdAdd(unsigned int id)
Definition: EsdCan.cpp:70
virtual bool canRead(CanBuffer &msgs, unsigned int size, unsigned int *read, bool wait=false)
Definition: EsdCan.cpp:88
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
Definition: ethParser.cpp:92
static pcap_t * handle
GLenum mode
Definition: rendering.cpp:48
fprintf(fid,'\n')