pcan_receive.cpp
Go to the documentation of this file.
1 // INCLUDE
2 
3 #include <stdio.h>
4 #include <stdlib.h>
5 #include <errno.h>
6 #include <unistd.h> // exit
7 #include <signal.h>
8 #include <string.h>
9 #include <stdlib.h> // strtoul
10 #include <fcntl.h> // O_RDWR
11 #include "common.h"
12 #include <ctype.h>
13 #include "ros/ros.h"
14 #include "std_msgs/String.h"
15 #include <sstream>
16 #include "pcan_receive.h"
17 
18 // DEFINES
19 
20 #define DEFAULT_NODE "/dev/pcan32"
21 
23 {
30  printf("pcan_receive - a small node, based on receivetest , which receives CAN messages and publishes them on a topic.\n");
31  printf("usage: pcan_receive {[-f=devicenode] | {[-b=BTR0BTR1] [-?]}}\n");
32  printf("options: -f - devicenode, default=%s\n", DEFAULT_NODE);
33  printf(" -b - BTR0BTR1 code in hex, e.g.\n");
34  printf(" 0x0014 = 1 MBit/s\n");
35  printf(" 0x001C = 500 kBit/s\n");
36  printf(" 0x011C = 250 kBit/s\n");
37  printf(" 0x031C = 125 kBit/s\n");
38  printf(" 0x432F = 100 kBit/s\n");
39  printf(" 0x472F = 50 kBit/s\n");
40  printf(" 0x532F = 20 kBit/s\n");
41  printf(" 0x672F = 10 kBit/s\n");
42  printf(" 0x7F7F = 5 kBit/s\n");
43  printf(" (default: 1 Mbit).\n");
44  printf(" -? or --help - this help\n");
45  printf("\n");
46 
47 }
48 
49 
51 {
58  //Initialisation
59  h = 0;
60  pcan_pub = n.advertise<std_msgs::String>("pcan_received", 1);
61 }
62 
63 
64 void pcan_receive::init(int argc, char **argv)
65 {
73  int nExtended = CAN_INIT_TYPE_ST;
74  int nType;
75  __u32 dwPort;
76  __u16 wIrq;
77  char *ptr;
78  __u16 wBTR0BTR1 = 0x0014;
79  // parameter wBTR0BTR1
80  // bitrate codes of BTR0/BTR1 registers
81  //#define CAN_BAUD_1M 0x0014 // 1 MBit/s
82  //#define CAN_BAUD_500K 0x001C // 500 kBit/s
83  //#define CAN_BAUD_250K 0x011C // 250 kBit/s
84  //#define CAN_BAUD_125K 0x031C // 125 kBit/s
85  //#define CAN_BAUD_100K 0x432F // 100 kBit/s
86  //#define CAN_BAUD_50K 0x472F // 50 kBit/s
87  //#define CAN_BAUD_20K 0x532F // 20 kBit/s
88  //#define CAN_BAUD_10K 0x672F // 10 kBit/s
89  //#define CAN_BAUD_5K 0x7F7F // 5 kBit/s
90  const char *szDevNode = DEFAULT_NODE;
91  bool bDevNodeGiven = false;
92  bool bTypeGiven = false;
93  char txt[VERSIONSTRING_LEN];
94  // decode command line arguments
95  for (int i = 1; i < argc; i++)
96  {
97  char c;
98  char *ptr;
99  ptr = argv[i];
100 
101  while (*ptr == '-')
102  ptr++;
103 
104  c = *ptr;
105  ptr++;
106 
107  if (*ptr == '=')
108  ptr++;
109 
110  switch(tolower(c))
111  {
112  case '?':
113  case 'h':
114  hlpMsg();
115  do_exit(errno, h);
116  break;
117  case 'f':
118  szDevNode = ptr;
119  bDevNodeGiven = true;
120  break;
121  case 'b':
122  wBTR0BTR1 = (__u16)strtoul(ptr, NULL, 16);
123  break;
124  default:
125  errno = EINVAL;
126  do_exit(errno, h);;
127  break;
128  }
129  }
130  /* open CAN port */
131  if ((bDevNodeGiven) || (!bDevNodeGiven && !bTypeGiven))
132  {
133  h = LINUX_CAN_Open(szDevNode, O_RDWR);
134  if (!h)
135  {
136  printf("pcan_receive: can't open %s\n", szDevNode);
137  do_exit(errno, h);;
138  }
139  }
140  else {
141  // please use what is appropriate
142  // HW_DONGLE_SJA
143  // HW_DONGLE_SJA_EPP
144  // HW_ISA_SJA
145  // HW_PCI
146  h = CAN_Open(nType, dwPort, wIrq);
147  if (!h)
148  {
149  printf("pcan_receive: can't open %s device.\n", getNameOfInterface(nType));
150  do_exit(errno, h);;
151  }
152  }
153  /* clear status */
154  CAN_Status(h);
155  // get version info
156  errno = CAN_VersionInfo(h, txt);
157  if (!errno)
158  printf("pcan_receive: driver version = %s\n", txt);
159  else {
160  perror("pcan_receive: CAN_VersionInfo()");
161  do_exit(errno, h);;
162  }
163  // init to a user defined bit rate
164  if (wBTR0BTR1)
165  {
166  errno = CAN_Init(h, wBTR0BTR1, 0x00);
167  if (errno)
168  {
169  perror("pcan_receive: CAN_Init()");
170  do_exit(errno, h );;
171  }
172  }
173 
174 }
176 {
184  // Real code
185  TPCANRdMsg m;
186  __u32 status;
187  if (LINUX_CAN_Read(h, &m))
188  {
189  perror("pcan_receive: LINUX_CAN_Read()");
190  }
191  else
192  {
193  //print_message_ex(&m);
194  // check if a CAN status is pending
195  if (m.Msg.MSGTYPE & MSGTYPE_STATUS)
196  {
197  status = CAN_Status(h);
198  if ((int)status < 0)
199  {
200  errno = nGetLastError();
201  perror("pcan_receive: CAN_Status()");
202  }
203  else
204  printf("pcan_receive: pending CAN status 0x%04x read.\n", (__u16)status);
205  }
206  }
207  pcan_pub.publish(TPCANRdMsgToString(m));
208 
209 
210 }
211 
212 
213 
214 
215 
216 std_msgs::String pcan_receive::TPCANRdMsgToString(TPCANRdMsg m)
217 {
225  std_msgs::String msg;
226  std::stringstream ss;
227  ss << m.dwTime << ".";
228  if(m.wUsec>=100)
229  {
230  ss << m.wUsec << " ";
231  }
232  else
233  {
234  if(m.wUsec>=10)
235  {
236  ss << "0" <<m.wUsec << " ";
237  }
238  else
239  {
240  ss << "00" <<m.wUsec << " ";
241  }
242  }
243  char IDENT[4];
244  sprintf(IDENT, "%02X", m.Msg.ID);
245  ss << "0x" ;
246  if(m.Msg.ID<16)
247  {
248  ss <<"0";
249  }
250  ss<< IDENT << " ";
251  char LEN[3];
252  sprintf(LEN, "%02X", m.Msg.LEN);
253  ss << "0x" << LEN << " ";
254  for (int i=0; i<sizeof(m.Msg.DATA); i++)
255  {
256  char DATA[3];
257  sprintf(DATA, "%02X", m.Msg.DATA[i]);
258  ss <<"0x" << DATA << " ";
259  }
260  msg.data=ss.str();
261  return msg;
262 }
std_msgs::String msg
Definition: pcan_receive.h:24
void init(int argc, char **argv)
void do_exit(int error, HANDLE h)
Definition: common.cpp:29
#define DEFAULT_NODE
char const * getNameOfInterface(int nType)
Definition: common.cpp:49
ros::Publisher pcan_pub
Definition: pcan_receive.h:23
ros::NodeHandle n
Definition: pcan_receive.h:22
std::stringstream ss
Definition: pcan_receive.h:25
std_msgs::String TPCANRdMsgToString(TPCANRdMsg m)


pcan_topics
Author(s):
autogenerated on Sun Dec 14 2014 19:01:10