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

Last change on this file since 81 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.