source: pacpussensors/trunk/CanGateway/driver/candump.cpp@ 116

Last change on this file since 116 was 4, checked in by guyardro, 11 years ago

add igep can driver for CanGateway and add support for igep compilation
File size: 9.1 KB
Line 
1#include <errno.h>
2#include <getopt.h>
3#include <libgen.h>
4#include <signal.h>
5#include <stdio.h>
6#include <stdlib.h>
7#include <string.h>
8#include <unistd.h>
9#include <limits.h>
10#include <stdint.h>
11
12#include <net/if.h>
13
14#include <sys/ioctl.h>
15#include <sys/socket.h>
16#include <sys/types.h>
17#include <sys/uio.h>
18
19#include <linux/can.h>
20#include <linux/can/raw.h>
21
22#include "BinaryDecoder.h"
23#include <ncurses.h>
24
25
26extern int optind, opterr, optopt;
27
28
29static int s = -1;
30static int running = 1;
31
32
33enum {
34// VERSION_OPTION = CHAR_MAX + 1,
35 FILTER_OPTION,
36};
37
38
39typedef struct{
40 float frontWheelsSpeed; // mean speed of the front wheels (in km/h)
41 float rearLeftWheelSpeed; // speed of the rear left wheel (in km/h)
42 float rearRightWheelSpeed; // speed of the rear right wheel (in km/h)
43 float rpmFrontWheels; // mean rpm of the front wheels (in tr/min)
44}StructWheelSpeed;
45
46
47static void print_usage(char *prg)
48{
49 fprintf(stderr, "Usage: %s [<can-interface>] [Options]\n"
50 "Options:\n"
51 " -f, --family=FAMILY\t" "protocol family (default PF_CAN = %d)\n"
52 " -t, --type=TYPE\t" "socket type, see man 2 socket (default SOCK_RAW = %d)\n"
53 " -p, --protocol=PROTO\t" "CAN protocol (default CAN_RAW = %d)\n"
54 " --filter=id:mask[:id:mask]...\n"
55 "\t\t\t" "apply filter\n"
56 " -h, --help\t\t" "this help\n"
57 " -o <filename>\t\t" "output into filename\n"
58 " -d\t\t\t" "daemonize\n"
59// " --version\t\t" "print version information and exit\n"
60 " -s, --speed\t\t" "decode the can frame 0x44D on Dyna and Carmen\n"
61 " -y, --dynamic_display\t" "dynamic display of the 0x44D on Dyna and Carmen\n"
62 " -a, --airplug\t\t" "allow frame's informations to be send to\n"
63 "\t\t\t" "airplug (not able with dynamic display)\n",
64 prg, PF_CAN, SOCK_RAW, CAN_RAW);
65}
66
67
68static void sigterm(int signo)
69{
70 running = 0;
71}
72
73
74static struct can_filter *filter_can = NULL;
75static int filter_count = 0;
76
77
78int add_filter(u_int32_t id, u_int32_t mask)
79{
80 filter_can = (can_filter*)realloc( filter_can, sizeof(struct can_filter ) * ( filter_count + 1));
81 if(!filter_can)
82 return -1;
83
84 filter_can[filter_count].can_id = id;
85 filter_can[filter_count].can_mask = mask;
86 filter_count++;
87
88 printf("id: 0x%08x mask: 0x%08x\n",id,mask);
89 return 0;
90}
91
92
93int add_filter_0x44d()
94{
95 add_filter( 0x44d, 0x44d );
96 return 0;
97}
98
99
100int add_dynamic_display()
101{
102 initscr();
103 return 0;
104}
105
106
107int add_airplug_communication()
108{
109
110
111 return 0;
112}
113
114
115#define BUF_SIZ (255)
116
117
118int main(int argc, char **argv)
119{
120 struct can_frame frame;
121 struct ifreq ifr;
122 struct sockaddr_can addr;
123 FILE *out = stdout;
124 char *interface = "can0";
125 char *optout = NULL;
126 char *ptr;
127 char buf[BUF_SIZ];
128
129 //test
130 unsigned char dataBuf[ 8 ];
131 unsigned short transform = 0;
132 StructWheelSpeed data_;
133
134 int family = PF_CAN, type = SOCK_RAW, proto = CAN_RAW;
135 int n = 0, err;
136 int nbytes, i;
137 int opt, optdaemon = 0;
138 int speed = 1, display = 1, airplug = 1;
139 float speedFront = 0.0, speedRearLeft = 0.0, speedRearRight = 0.0, speedWheel = 0.0;
140 uint32_t id, mask;
141
142 signal(SIGPIPE, SIG_IGN);
143
144 struct option long_options[] = {
145 { "help", no_argument, 0, 'h' },
146 { "speed", no_argument, 0, 's' },
147 { "dynamic_display", no_argument, 0, 'y' },
148 { "airplug", no_argument, 0, 'a' },
149 { "family", required_argument, 0, 'f' },
150 { "protocol", required_argument, 0, 'p' },
151 { "type", required_argument, 0, 't' },
152 { "filter", required_argument, 0, FILTER_OPTION },
153// { "version", no_argument, 0, VERSION_OPTION},
154 { 0, 0, 0, 0},
155 };
156
157 while ((opt = getopt_long(argc, argv, "f:t:p:o:d:ayhs", long_options, NULL)) != -1) {
158 switch (opt) {
159 case 'd':
160 optdaemon++;
161 break;
162
163 case 'h':
164 print_usage(basename(argv[0]));
165 exit(0);
166
167 case 'f':
168 family = strtoul(optarg, NULL, 0);
169 break;
170
171 case 't':
172 type = strtoul(optarg, NULL, 0);
173 break;
174
175 case 'p':
176 proto = strtoul(optarg, NULL, 0);
177 break;
178
179 case 'o':
180 optout = optarg;
181 break;
182
183 case FILTER_OPTION:
184 ptr = optarg;
185 while(1) {
186 id = strtoul(ptr, NULL, 0);
187 ptr = strchr(ptr, ':');
188 if(!ptr) {
189 fprintf(stderr, "filter must be applied in the form id:mask[:id:mask]...\n");
190 exit(1);
191 }
192 ptr++;
193 mask = strtoul(ptr, NULL, 0);
194 ptr = strchr(ptr, ':');
195 add_filter(id,mask);
196 if(!ptr)
197 break;
198 ptr++;
199 }
200 break;
201
202 case 's':
203 speed = add_filter_0x44d();
204 break;
205
206 case 'y':
207 if ( airplug != 0 ) {
208 speed = add_filter_0x44d();
209 display = add_dynamic_display();
210 } else {
211 endwin();
212 printf("Can not activate airplug communication with the dynamic display\nProgram will exit\n");
213 exit(1);
214 }
215 break;
216
217 case 'a':
218 if ( display != 0 ) {
219 speed = add_filter_0x44d();
220 airplug = 0;
221 } else {
222 endwin();
223 printf("Can not activate airplug communication with the dynamic display\nProgram will exit\n");
224 exit(1);
225 }
226 break;
227
228// case VERSION_OPTION:
229// printf("candump %s\n",VERSION);
230// exit(0);
231
232 default:
233 fprintf(stderr, "Unknown option %c\n", opt);
234 break;
235 }
236 }
237
238 if (optind != argc)
239 interface = argv[optind];
240 printf("interface = %s, family = %d, type = %d, proto = %d\n", interface, family, type, proto);
241
242 if ((s = socket(family, type, proto)) < 0) {
243 perror("socket");
244 return 1;
245 }
246
247 addr.can_family = family;
248 strncpy(ifr.ifr_name, interface, sizeof(ifr.ifr_name));
249
250 if (ioctl(s, SIOCGIFINDEX, &ifr)) {
251 perror("ioctl");
252 return 1;
253 }
254 addr.can_ifindex = ifr.ifr_ifindex;
255
256 if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
257 perror("bind");
258 return 1;
259 }
260
261 if (filter_can) {
262 if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, filter_can,
263 filter_count * sizeof(struct can_filter)) != 0) {
264 perror("setsockopt");
265 exit(1);
266 }
267 }
268
269 if (optdaemon)
270 daemon(1, 0);
271 else {
272 signal(SIGTERM, sigterm);
273 signal(SIGHUP, sigterm);
274 }
275
276 if (optout) {
277 out = fopen(optout, "a");
278 if (!out) {
279 perror("fopen");
280 exit (EXIT_FAILURE);
281 }
282 }
283
284 while (running) {
285 if ((nbytes = read(s, &frame, sizeof(struct can_frame))) < 0) {
286 perror("read");
287 return 1;
288 } else {
289 if (frame.can_id & CAN_EFF_FLAG)
290 n = snprintf(buf, BUF_SIZ, "<0x%08x> ", frame.can_id & CAN_EFF_MASK);
291 else
292 n = snprintf(buf, BUF_SIZ, "<0x%03x> ", frame.can_id & CAN_SFF_MASK);
293
294 n += snprintf(buf + n, BUF_SIZ - n, "[%d] ", frame.can_dlc);
295 for (i = 0; i < frame.can_dlc; i++) {
296 n += snprintf(buf + n, BUF_SIZ - n, "%02x ", frame.data[i]);
297 }
298 if (frame.can_id & CAN_RTR_FLAG)
299 n += snprintf(buf + n, BUF_SIZ - n, "remote request");
300
301 if ( speed == 0 ) {
302
303 memcpy( dataBuf, frame.data, sizeof( dataBuf ) );
304
305// speedFront = ( ( ( int )frame.data[ 0 ] ) * 256 + ( int )frame.data[ 1 ] ) * 0.01;
306// speedRearLeft = ( ( ( int )frame.data[ 2 ] ) * 256 + ( int )frame.data[ 3 ] ) * 0.01;
307// speedRearRight = ( ( ( int )frame.data[ 4 ] ) * 256 + ( int )frame.data[ 5 ] ) * 0.01;
308// speedWheel = ( ( ( int )frame.data[ 6 ] ) * 256 + ( int )frame.data[ 7 ] ) * 0.04;
309 if ( mDecodeToUI16(&transform, dataBuf, 7, 16) )
310 data_.frontWheelsSpeed = static_cast<float>(transform * 0.01);
311 if ( mDecodeToUI16(&transform, dataBuf, 23, 16) )
312 data_.rearLeftWheelSpeed = static_cast<float>(transform * 0.01);
313 if ( mDecodeToUI16(&transform, dataBuf, 39, 16) )
314 data_.rearRightWheelSpeed = static_cast<float>(transform * 0.01);
315 if ( mDecodeToUI16(&transform, dataBuf, 55, 16) )
316 data_.rpmFrontWheels = static_cast<float>(transform * 0.04);
317
318 if ( display == 0 ) {
319 move( 0, 0);
320 printw( "Vitesse avant : %.2f km/h\n", data_.frontWheelsSpeed );
321 move( 1, 0);
322 printw( "Vitesse arriere gauche : %.2f km/h\n", data_.rearLeftWheelSpeed );
323 move( 2, 0);
324 printw( "Vitesse arriere droite : %.2f km/h\n", data_.rearRightWheelSpeed );
325 move( 3, 0);
326 printw( "Vitesse roue : %.2f tr/min\n", data_.rpmFrontWheels );
327 refresh();
328 } else if ( airplug == 0 ) {
329
330 printf( "^pacpl~\\!f~t!T~SpeedFront!t~0!d~%.2f!\\!f~t!T~SpeedRearLeft!t~0!d~%.2f!\\!f~t!T~SpeedRearRight!t~0!d~%.2f!\\!f~t!T~SpeedWheel!t~0!d~%.2f!\\^\n", data_.frontWheelsSpeed, data_.rearLeftWheelSpeed, data_.rearRightWheelSpeed, data_.rpmFrontWheels );
331 } else {
332 fprintf( out, "Vitesse avant : %.2f km/h\n", data_.frontWheelsSpeed );
333 fprintf( out, "Vitesse arriere gauche : %.2f km/h\n", data_.rearLeftWheelSpeed );
334 fprintf( out, "Vitesse arriere droite : %.2f km/h\n", data_.rearRightWheelSpeed );
335 fprintf( out, "Vitesse roue : %.2f tr/min\n", data_.rpmFrontWheels );
336 }
337
338 } else {
339 fprintf(out, "%s\n", buf);
340 }
341
342 do {
343 err = fflush(out);
344 if (err == -1 && errno == EPIPE) {
345 err = -EPIPE;
346 fclose(out);
347 out = fopen(optout, "a");
348 if (!out)
349 exit (EXIT_FAILURE);
350 }
351 } while (err == -EPIPE);
352
353 n = 0;
354 }
355 }
356
357 endwin();
358
359 exit (EXIT_SUCCESS);
360}
Note: See TracBrowser for help on using the repository browser.