14 #include "std_msgs/String.h"
20 #define DEFAULT_NODE "/dev/pcan32"
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");
60 pcan_pub =
n.advertise<std_msgs::String>(
"pcan_received", 1);
73 int nExtended = CAN_INIT_TYPE_ST;
78 __u16 wBTR0BTR1 = 0x0014;
91 bool bDevNodeGiven =
false;
92 bool bTypeGiven =
false;
93 char txt[VERSIONSTRING_LEN];
95 for (
int i = 1; i < argc; i++)
119 bDevNodeGiven =
true;
122 wBTR0BTR1 = (__u16)strtoul(ptr, NULL, 16);
131 if ((bDevNodeGiven) || (!bDevNodeGiven && !bTypeGiven))
133 h = LINUX_CAN_Open(szDevNode, O_RDWR);
136 printf(
"pcan_receive: can't open %s\n", szDevNode);
146 h = CAN_Open(nType, dwPort, wIrq);
156 errno = CAN_VersionInfo(
h, txt);
158 printf(
"pcan_receive: driver version = %s\n", txt);
160 perror(
"pcan_receive: CAN_VersionInfo()");
166 errno = CAN_Init(
h, wBTR0BTR1, 0x00);
169 perror(
"pcan_receive: CAN_Init()");
187 if (LINUX_CAN_Read(
h, &m))
189 perror(
"pcan_receive: LINUX_CAN_Read()");
195 if (m.Msg.MSGTYPE & MSGTYPE_STATUS)
197 status = CAN_Status(
h);
200 errno = nGetLastError();
201 perror(
"pcan_receive: CAN_Status()");
204 printf(
"pcan_receive: pending CAN status 0x%04x read.\n", (__u16)status);
225 std_msgs::String
msg;
226 std::stringstream
ss;
227 ss << m.dwTime <<
".";
230 ss << m.wUsec <<
" ";
236 ss <<
"0" <<m.wUsec <<
" ";
240 ss <<
"00" <<m.wUsec <<
" ";
244 sprintf(IDENT,
"%02X", m.Msg.ID);
252 sprintf(LEN,
"%02X", m.Msg.LEN);
253 ss <<
"0x" << LEN <<
" ";
254 for (
int i=0; i<
sizeof(m.Msg.DATA); i++)
257 sprintf(DATA,
"%02X", m.Msg.DATA[i]);
258 ss <<
"0x" << DATA <<
" ";
void init(int argc, char **argv)
void do_exit(int error, HANDLE h)
char const * getNameOfInterface(int nType)
std_msgs::String TPCANRdMsgToString(TPCANRdMsg m)