5 * @brief CAN-ORTE bridge
7 * Copyright: (c) 2008 CTU Dragons
8 * CTU FEE - Department of Control Engineering
19 #include <sys/socket.h>
20 #include <sys/ioctl.h>
27 #include <can_msg_masks.h>
28 #include <can_msg_def.h>
32 #include <roboorte_robottype.h>
33 #include <can_msg_masks.h>
38 if ((sock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
39 perror("cand: unable to create a socket");
43 can_addr.can_family = AF_CAN;
44 std::strcpy(can_ifreq.ifr_name, "can0");
46 if (ioctl(sock, SIOCGIFINDEX, &can_ifreq) < 0) {
47 perror("cand: SIOCGIFINDEX");
51 can_addr.can_ifindex = can_ifreq.ifr_ifindex;
53 if (!can_addr.can_ifindex) {
54 perror("cand: invalid socket interface");
58 if (bind(sock, (struct sockaddr *)&can_addr, sizeof(can_addr)) < 0) {
59 perror("cand: unable to bind");
66 int set_pwr_ctrl(struct robottype_orte_data *orte_data)
69 if(orte_data->pwr_ctrl.voltage33)
70 data |= CAN_PWR_CTRL_33_ON;
72 data |= CAN_PWR_CTRL_33_OFF;
74 if(orte_data->pwr_ctrl.voltage50)
75 data |= CAN_PWR_CTRL_50_ON;
77 data |= CAN_PWR_CTRL_50_OFF;
79 if(orte_data->pwr_ctrl.voltage80)
80 data |= CAN_PWR_CTRL_80_ON;
82 data |= CAN_PWR_CTRL_80_OFF;
84 can_send(CAN_PWR, 1, &data);
89 int set_pwr_alert(struct robottype_orte_data *orte_data)
92 data = orte_data->pwr_alert.value;
94 can_send(CAN_PWR_ALERT, 1, &data);
99 int send_can_msg(struct robottype_orte_data *orte_data)
103 data[0] = orte_data->can_msg.data1;
104 data[1] = orte_data->can_msg.data2;
105 data[2] = orte_data->can_msg.data3;
106 data[3] = orte_data->can_msg.data4;
107 data[4] = orte_data->can_msg.data5;
108 data[5] = orte_data->can_msg.data6;
109 data[6] = orte_data->can_msg.data7;
110 data[7] = orte_data->can_msg.data8;
112 can_send(orte_data->can_msg.id, orte_data->can_msg.len, data);
115 int set_motor_speed(struct robottype_orte_data *orte_data)
117 unsigned char data[4];
119 data[0] = orte_data->motion_speed.right >> 8;
120 data[1] = orte_data->motion_speed.right & 0xff;
121 data[2] = orte_data->motion_speed.left >> 8;
122 data[3] = orte_data->motion_speed.left & 0xff;
123 can_send(CAN_MOTION_CMD, 4, data);
126 for (int i=0; i< 4; i++) {
127 printf("%02x ",data[i]);
135 * Sends #CAN_SERVO message.
137 * - data[3] = orte_data->servos.reserve;
138 * - data[2] = orte_data->servos.holder;
139 * - data[1] = orte_data->servos.brush_right;
140 * - data[0] = orte_data->servos.brush_left;
142 int set_serva(struct robottype_orte_data *orte_data)
144 unsigned char data[4];
146 data[3] = orte_data->servos.reserve;
147 data[2] = orte_data->servos.holder;
148 data[1] = orte_data->servos.brush_right;
149 data[0] = orte_data->servos.brush_left;
150 can_send(CAN_SERVO, 4, data);
155 int set_laser_cmd(struct robottype_orte_data *orte_data)
157 unsigned char data[1];
159 data[0] = orte_data->laser_cmd.speed;
160 can_send(CAN_LAS_CMD, 1, data);
165 int set_drives(struct robottype_orte_data *orte_data)
167 unsigned char data[4];
169 data[3] = orte_data->drives.pusher;
170 data[2] = orte_data->drives.lift_pos;
171 data[1] = orte_data->drives.brush_right;
172 data[0] = orte_data->drives.brush_left;
173 can_send(CAN_DRIVES, 4, data);
177 int can_send(canid_t id, unsigned char length, unsigned char *data)
179 struct can_frame frame;
183 frame.can_dlc = length;
185 for(int i=0; i<length; i++)
186 frame.data[i] = data[i];
188 if ((size = write(sock, &frame, sizeof(struct can_frame))) < 0) {
189 perror("cand: can send: write()");
191 } else if (size < sizeof(struct can_frame)) {
192 perror("cand: can send: incomplete CAN frame\n");
197 printf("write returned %d\n", size);
198 printf("finnished can send\n");
205 * Parse frame ID and react as required
207 int cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
214 unsigned short *las_meas;
215 int las_mi, las_di, las_bcnt;
216 static unsigned short prev_meas;
219 switch(frame.can_id) {
220 /* voltage measurements from power board */
222 /* robot commands (start, ..) */
224 orte->robot_cmd.start = frame.data[0];
225 orte->robot_cmd.stop = frame.data[1];
226 ORTEPublicationSend(orte->publication_robot_cmd);
229 /* global sampling period -- time trigger */
231 orte->corr_trig.seq = frame.data[0];
232 ORTEPublicationSend(orte->publication_corr_trigger);
235 /* distances measured by ULoPoS */
237 orte->corr_trig.t1 = (frame.data[1]<<8)|(frame.data[0]);
238 orte->corr_trig.t2 = (frame.data[3]<<8)|(frame.data[2]);
239 orte->corr_trig.t3 = (frame.data[5]<<8)|(frame.data[4]);
240 ORTEPublicationSend(orte->publication_corr_distances);
243 /* positioning by odometry */
244 case CAN_MOTION_ODOMETRY_SIMPLE:
245 orte->motion_irc.right =
246 ((frame.data[0]<<24)|(frame.data[1]<<16)|
247 (frame.data[2]<<8)|(frame.data[3]));
248 orte->motion_irc.left =
249 ((frame.data[4]<<24)|(frame.data[5]<<16)|
250 (frame.data[6]<<8)|(frame.data[7]));
251 ORTEPublicationSend(orte->publication_motion_irc);
255 case CAN_MOTION_STATUS:
256 orte->motion_status.err_left =
257 (frame.data[0]<<8)|(frame.data[1]);
258 orte->motion_status.err_right =
259 (frame.data[2]<<8)|(frame.data[3]);
260 if(++status_cnt == 5) {
261 ORTEPublicationSend(orte->publication_motion_status);
266 double volt33, voltBAT;
267 voltBAT = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
268 (frame.data[2] << 8) | (frame.data[3]))/10000.0;
269 volt33 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
270 (frame.data[6] << 8) | (frame.data[7]))/10000.0;
271 // printf("PWR_ADC1: 3,3V = %2.2f, BAT = %2.2f\n",volt33,voltBAT);
272 orte->pwr_voltage.voltage33 = volt33;
273 orte->pwr_voltage.voltageBAT = voltBAT;
277 double volt80, volt50;
278 volt50 = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
279 (frame.data[2] << 8) | (frame.data[3]))/10000.0;
280 volt80 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
281 (frame.data[6] << 8) | (frame.data[7]))/10000.0;
282 // printf("PWR_ADC2: 8,0V = %2.2f, 5,0V = %2.2f\n",volt80,volt50);
283 orte->pwr_voltage.voltage80 = volt80;
284 orte->pwr_voltage.voltage50 = volt50;
286 ORTEPublicationSend(orte->publication_pwr_voltage);
290 orte->sharps.front_right = s_gp2y0a21_ir2mmLong((frame.data[0] << 8)| \
291 (frame.data[1]))/1000.0;
292 orte->sharps.right = s_gp2y0a21_ir2mmLong((frame.data[2] << 8)| \
293 (frame.data[3]))/1000.0;
294 orte->sharps.left = s_gp2y0a21_ir2mmLong((frame.data[4] << 8)| \
295 (frame.data[5]))/1000.0;
296 orte->sharps.front_left = s_gp2y0a21_ir2mmLong((frame.data[6] << 8)| \
297 (frame.data[7]))/1000.0;
298 ORTEPublicationSend(orte->publication_sharps);
302 orte->sharps.back_left = s_gp2d120_ir2mmShort((frame.data[4] << 8)| \
303 (frame.data[5]))/1000.0;
304 orte->sharps.back_right = s_gp2d120_ir2mmShort((frame.data[6] << 8)| \
305 (frame.data[7]))/1000.0;
306 ORTEPublicationSend(orte->publication_sharps);
311 // for (i=0; i<frame.can_dlc; i++) {
312 // printf("0x%02x ", frame.data[i]);
314 // printf("can_dlc=%d\n", frame.can_dlc);
315 orte->laser_meas.cnt = frame.data[1];
316 las_bcnt = orte->laser_meas.cnt;
317 last_id = frame.data[0];
319 /* rotation period */
320 orte->laser_meas.period = (frame.data[2]<<8)|(frame.data[3]);
321 // printf("CAN ID=0x%02x: cnt=%d period=%d measures: ",
322 // frame.can_id, orte->laser_meas.cnt, orte->laser_meas.period);
324 for (las_mi=0; las_mi<(frame.can_dlc-4)/2; las_mi++) {
326 case 0: las_meas = &orte->laser_meas.measures0; break;
327 case 1: las_meas = &orte->laser_meas.measures1; break;
331 (frame.data[las_di++]<<8)|(frame.data[las_di++]);
332 // printf("0x%02x 0x%02x %u ",
333 // frame.data[las_di-2], frame.data[las_di-1], *las_meas);
341 // for (i=0; i<frame.can_dlc; i++) {
342 // printf("0x%02x ", frame.data[i]);
344 // printf("can_dlc=%d\n", frame.can_dlc);
345 // printf("CAN ID=0x%02x: cnt=%d period=%d measures: ",
346 // frame.can_id, orte->laser_meas.cnt, orte->laser_meas.period);
347 if (frame.data[0] != (last_id+(frame.can_id-CAN_LAS1)))
350 while (las_di < 8 && las_bcnt > 0) {
352 case 2: las_meas = &orte->laser_meas.measures2; break;
353 case 3: las_meas = &orte->laser_meas.measures3; break;
354 case 4: las_meas = &orte->laser_meas.measures4; break;
355 case 5: las_meas = &orte->laser_meas.measures5; break;
356 case 6: las_meas = &orte->laser_meas.measures6; break;
357 case 7: las_meas = &orte->laser_meas.measures7; break;
358 case 8: las_meas = &orte->laser_meas.measures8; break;
359 case 9: las_meas = &orte->laser_meas.measures9; break;
363 (frame.data[las_di++]<<8)|(frame.data[las_di++]);
364 // printf("0x%02x 0x%02x %u ",
365 // frame.data[las_di-2], frame.data[las_di-1], *las_meas);
371 // printf("ORTEPublicationSend()\n");
372 ORTEPublicationSend(orte->publication_laser_meas);
377 orte->laser_data.period = (frame.data[0]<<8)|(frame.data[1]);
378 orte->laser_data.measure = (frame.data[2]<<8)|(frame.data[3]);
379 if (prev_meas != orte->laser_data.measure) {
380 ORTEPublicationSend(orte->publication_laser_data);
381 prev_meas = orte->laser_data.measure;
386 orte->cmu.color = frame.data[0];
387 ORTEPublicationSend(orte->publication_cmu);
391 orte->bumper.left = frame.data[0] & LEFT_BUMPER;
392 orte->bumper.right = frame.data[0] & RIGHT_BUMPER;
393 ORTEPublicationSend(orte->publication_bumper);
397 // printf("received CAN msg with unknown id: %x\n",frame.can_id);
402 void rcv_pwr_ctrl_cb (const ORTERecvInfo *info, void *vinstance,
403 void *recvCallBackParam)
405 struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
407 switch (info->status) {
409 set_pwr_ctrl(orte_data);
412 //printf("ORTE deadline occurred - PWR_CTRL receive\n");
417 void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
418 void *recvCallBackParam)
420 struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
422 switch (info->status) {
424 send_can_msg(orte_data);
427 //printf("ORTE deadline occurred - PWR_CTRL receive\n");
431 void rcv_pwr_alert_cb (const ORTERecvInfo *info, void *vinstance,
432 void *recvCallBackParam)
435 struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
436 switch (info->status) {
438 set_pwr_alert(orte_data);
441 //printf("ORTE deadline occurred - PWR_CTRL receive\n");
446 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
447 void *recvCallBackParam)
449 struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
451 switch (info->status) {
453 /* reversing motion direction, as it is different, than last year */
454 orte_data->motion_speed.left *= -1;
455 orte_data->motion_speed.right *=-1;
456 set_motor_speed(orte_data);
457 /*printf("motor cmd received\n");*/
460 printf("motor cmd deadline occurred, stopping motors\n");
461 orte_data->motion_speed.left = 0;
462 orte_data->motion_speed.right = 0;
463 set_motor_speed(orte_data);
468 void rcv_servos_cb (const ORTERecvInfo *info, void *vinstance,
469 void *recvCallBackParam)
471 struct robottype_orte_data *orte_data =
472 (struct robottype_orte_data *)recvCallBackParam;
474 switch (info->status) {
476 set_serva(orte_data);
479 // printf("ORTE deadline occurred - servo receive\n");
484 void rcv_laser_cmd_cb (const ORTERecvInfo *info, void *vinstance,
485 void *recvCallBackParam)
487 struct robottype_orte_data *orte_data =
488 (struct robottype_orte_data *)recvCallBackParam;
490 switch (info->status) {
492 set_laser_cmd(orte_data);
495 // printf("ORTE deadline occurred - servo receive\n");
500 void rcv_drives_cb (const ORTERecvInfo *info, void *vinstance,
501 void *recvCallBackParam)
503 struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
505 switch (info->status) {
507 set_drives(orte_data);
510 // printf("ORTE deadline occurred - drives receive\n");
515 int main(int argc, char *argv[])
518 struct timeval timeout;
523 struct robottype_orte_data orte;
524 struct can_frame frame;
526 if (cand_init() < 0) {
527 printf("cand: init failed\n");
530 printf("cand: init OK\n");
535 /* orte initialization */
536 robottype_roboorte_init(&orte);
538 /* creating publishers */
539 robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
540 robottype_publisher_motion_status_create(&orte, NULL, NULL);
541 robottype_publisher_motion_irc_create(&orte, NULL, NULL);
542 robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
543 robottype_publisher_laser_meas_create(&orte, NULL, NULL);
544 robottype_publisher_laser_data_create(&orte, NULL, NULL);
545 robottype_publisher_sharps_create(&orte, NULL, NULL);
546 robottype_publisher_cmu_create(&orte, NULL, NULL);
547 robottype_publisher_bumper_create(&orte, NULL, NULL);
549 /* creating subscribers */
550 robottype_subscriber_pwr_ctrl_create(&orte,
551 rcv_pwr_ctrl_cb, &orte);
552 robottype_subscriber_pwr_alert_create(&orte,
553 rcv_pwr_alert_cb, &orte);
554 robottype_subscriber_motion_speed_create(&orte,
555 rcv_motion_speed_cb, &orte);
556 robottype_subscriber_can_msg_create(&orte,
557 rcv_can_msg_cb, &orte);
558 robottype_subscriber_servos_create(&orte,
559 rcv_servos_cb, &orte);
560 robottype_subscriber_laser_cmd_create(&orte,
561 rcv_laser_cmd_cb, &orte);
563 robottype_subscriber_drives_create(&orte,
564 rcv_drives_cb, &orte);
567 FD_ZERO(&read_fd_set);
568 FD_SET(sock, &read_fd_set);
570 timeout.tv_usec = 10000;
572 ret = select(FD_SETSIZE, &read_fd_set, NULL, NULL, &timeout);
574 perror("cand: select() error");
578 /* timeout expired, nothing to be done */
582 if (!FD_ISSET(sock, &read_fd_set))
585 /* read data from SocketCAN */
586 size = read(sock, &frame, sizeof(struct can_frame));
589 cand_parse_frame(&orte, frame);