src/cand/cand.cc

   1 /**
   2  * @file   cand.cc
   3  * @date   08/04/08
   4  *
   5  * @brief  CAN-ORTE bridge
   6  *
   7  * Copyright: (c) 2008 CTU Dragons
   8  *            CTU FEE - Department of Control Engineering
   9  * License: GNU GPL v.2
  10  */
  11
  12 #ifdef HAVE_CONFIG_H
  13 #include <config.h>
  14 #endif
  15
  16 #include <cstring>
  17 #include <iostream>
  18 #include <cstdlib>
  19 #include <sys/socket.h>
  20 #include <sys/ioctl.h>
  21 #include <fcntl.h>
  22
  23 #include <af_can.h>
  24 #include <canutils.h>
  25
  26 #include <can_ids.h>
  27 #include <can_msg_masks.h>
  28 #include <can_msg_def.h>
  29 #include <sharp.h>
  30 #include <orte.h>
  31 #include <roboorte.h>
  32 #include <roboorte_robottype.h>
  33 #include <can_msg_masks.h>
  34 #include "cand.h"
  35
  36 int cand_init()
  37 {
  38         if ((sock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
  39                 perror("cand: unable to create a socket");
  40                 return -1;
  41         }
  42
  43         can_addr.can_family = AF_CAN;
  44         std::strcpy(can_ifreq.ifr_name, "can0");
  45
  46         if (ioctl(sock, SIOCGIFINDEX, &can_ifreq) < 0) {
  47                 perror("cand: SIOCGIFINDEX");
  48                 return -2;
  49         }
  50
  51         can_addr.can_ifindex = can_ifreq.ifr_ifindex;
  52
  53         if (!can_addr.can_ifindex) {
  54                 perror("cand: invalid socket interface");
  55                 return -3;
  56         }
  57
  58         if (bind(sock, (struct sockaddr *)&can_addr, sizeof(can_addr)) < 0) {
  59                 perror("cand: unable to bind");
  60                 return -4;
  61         }
  62
  63         return 0;
  64 }
  65
  66 int set_pwr_ctrl(struct robottype_orte_data *orte_data)
  67 {
  68         unsigned char data=0;
  69         if(orte_data->pwr_ctrl.voltage33)
  70                 data |= CAN_PWR_CTRL_33_ON;
  71         else
  72                 data |= CAN_PWR_CTRL_33_OFF;
  73
  74         if(orte_data->pwr_ctrl.voltage50)
  75                 data |= CAN_PWR_CTRL_50_ON;
  76         else
  77                 data |= CAN_PWR_CTRL_50_OFF;
  78
  79         if(orte_data->pwr_ctrl.voltage80)
  80                 data |= CAN_PWR_CTRL_80_ON;
  81         else
  82                 data |= CAN_PWR_CTRL_80_OFF;
  83
  84         can_send(CAN_PWR, 1, &data);
  85
  86         return 0;
  87 }
  88
  89 int set_pwr_alert(struct robottype_orte_data *orte_data)
  90 {
  91         unsigned char data=0;
  92         data = orte_data->pwr_alert.value;
  93
  94         can_send(CAN_PWR_ALERT, 1, &data);
  95
  96         return 0;
  97 }
  98
  99 int send_can_msg(struct robottype_orte_data *orte_data)
 100 {
 101         uint8_t data[8];
 102
 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;
 111
 112         can_send(orte_data->can_msg.id, orte_data->can_msg.len, data);
 113 }
 114
 115 int set_motor_speed(struct robottype_orte_data *orte_data)
 116 {
 117         unsigned char data[4];
 118
 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);
 124
 125         /*printf("data: ");
 126         for (int i=0; i< 4; i++) {
 127                 printf("%02x ",data[i]);
 128         }
 129         printf("\n");*/
 130
 131         return 0;
 132 }
 133
 134 /**
 135  * Sends #CAN_SERVO message.
 136  *
 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;
 141  */
 142 int set_serva(struct robottype_orte_data *orte_data)
 143 {
 144         unsigned char data[4];
 145
 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);
 151
 152         return 0;
 153 }
 154
 155 int set_laser_cmd(struct robottype_orte_data *orte_data)
 156 {
 157         unsigned char data[1];
 158
 159         data[0] = orte_data->laser_cmd.speed;
 160         can_send(CAN_LAS_CMD, 1, data);
 161
 162         return 0;
 163 }
 164
 165 int set_drives(struct robottype_orte_data *orte_data)
 166 {
 167         unsigned char data[4];
 168
 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);
 174
 175         return 0;
 176 }
 177 int can_send(canid_t id, unsigned char length, unsigned char *data)
 178 {
 179         struct can_frame frame;
 180         int size;
 181
 182         frame.can_id = id;
 183         frame.can_dlc = length;
 184
 185         for(int i=0; i<length; i++)
 186                 frame.data[i] = data[i];
 187
 188         if ((size = write(sock, &frame, sizeof(struct can_frame))) < 0) {
 189                 perror("cand: can send: write()");
 190                 return -1;
 191         } else if (size < sizeof(struct can_frame)) {
 192                 perror("cand: can send: incomplete CAN frame\n");
 193                 return 1;
 194         }
 195
 196         /*
 197         printf("write returned %d\n", size);
 198         printf("finnished can send\n");
 199         */
 200
 201         return 0;
 202 }
 203
 204 /**
 205  * Parse frame ID and react as required
 206  */
 207 int cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
 208 {
 209         int status_cnt = 0;
 210         int adc1_cnt = 0;
 211         int adc2_cnt = 0;
 212         int adc3_cnt = 0;
 213         int ir_cnt = 0;
 214         unsigned short *las_meas;
 215         int las_mi, las_di, las_bcnt;
 216         static unsigned short prev_meas;
 217         int i;
 218
 219         switch(frame.can_id) {
 220                 /* voltage measurements from power board */
 221
 222                 /* robot commands (start, ..) */
 223                 case CAN_ROBOT_CMD:
 224                         orte->robot_cmd.start = frame.data[0];
 225                         orte->robot_cmd.stop = frame.data[1];
 226                         ORTEPublicationSend(orte->publication_robot_cmd);
 227                         break;
 228
 229                 /* global sampling period -- time trigger */
 230                 case CAN_CORR_TRIG:
 231                         orte->corr_trig.seq = frame.data[0];
 232                         ORTEPublicationSend(orte->publication_corr_trig);
 233                         break;
 234
 235                 /* distances measured by ULoPoS */
 236                 case CAN_CORR_DIST:
 237                         orte->corr_distances.t1 = (frame.data[1]<<8)|(frame.data[0]);
 238                         orte->corr_distances.t2 = (frame.data[3]<<8)|(frame.data[2]);
 239                         orte->corr_distances.t3 = (frame.data[5]<<8)|(frame.data[4]);
 240                         ORTEPublicationSend(orte->publication_corr_distances);
 241                         break;
 242
 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);
 252                         break;
 253
 254                 /* motion status */
 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);
 262                                 status_cnt = 0;
 263                         }
 264                         break;
 265                 case CAN_PWR_ADC1:
 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;
 274                         break;
 275
 276                 case CAN_PWR_ADC2:
 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;
 285
 286                         ORTEPublicationSend(orte->publication_pwr_voltage);
 287
 288                         break;
 289                 case CAN_ADC_1:
 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);
 299                         break;
 300
 301                 case CAN_ADC_2:
 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);
 307                         break;
 308                 /* laser data */
 309                 case CAN_LAS1:
 310 //                      printf("CAN: ");
 311 //                      for (i=0; i<frame.can_dlc; i++) {
 312 //                              printf("0x%02x ", frame.data[i]);
 313 //                      }
 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];
 318                         las_di = 4;
 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);
 323
 324                         for (las_mi=0; las_mi<(frame.can_dlc-4)/2; las_mi++) {
 325                                 switch (las_mi) {
 326                                         case 0: las_meas = &orte->laser_meas.measures0; break;
 327                                         case 1: las_meas = &orte->laser_meas.measures1; break;
 328                                         default: break;
 329                                 }
 330                                 *las_meas =
 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);
 334                         }
 335 //                      printf("\n");
 336                         break;
 337                 case CAN_LAS2:
 338                 case CAN_LAS3:
 339                 case CAN_LAS4:
 340 //                      printf("CAN: ");
 341 //                      for (i=0; i<frame.can_dlc; i++) {
 342 //                              printf("0x%02x ", frame.data[i]);
 343 //                      }
 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)))
 348                                 break;
 349                         las_di = 2;
 350                         while (las_di < 8 && las_bcnt > 0) {
 351                                 switch (las_mi) {
 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;
 360                                         default: break;
 361                                 }
 362                                 *las_meas =
 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);
 366                                 las_mi++;
 367                                 las_bcnt--;
 368                         }
 369                         printf("\n");
 370                         if (las_bcnt == 0) {
 371 //                              printf("ORTEPublicationSend()\n");
 372                                 ORTEPublicationSend(orte->publication_laser_meas);
 373                         }
 374                         break;
 375
 376                 case CAN_LAS_DATA:
 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;
 382                         }
 383                         break;
 384
 385                 case CAN_CMU:
 386                         orte->cmu.color = frame.data[0];
 387                         ORTEPublicationSend(orte->publication_cmu);
 388                         break;
 389
 390                 case CAN_BUMPER:
 391                         orte->bumper.left = frame.data[0] & LEFT_BUMPER;
 392                         orte->bumper.right = frame.data[0] & RIGHT_BUMPER;
 393                         ORTEPublicationSend(orte->publication_bumper);
 394                         break;
 395
 396                 default:
 397 //                      printf("received CAN msg with unknown id: %x\n",frame.can_id);
 398                         break;
 399         }
 400 }
 401
 402 void rcv_pwr_ctrl_cb (const ORTERecvInfo *info, void *vinstance,
 403                         void *recvCallBackParam)
 404 {
 405         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 406
 407         switch (info->status) {
 408                 case NEW_DATA:
 409                         set_pwr_ctrl(orte_data);
 410                         break;
 411                 case DEADLINE:
 412                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 413                         break;
 414         }
 415 }
 416
 417 void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
 418                         void *recvCallBackParam)
 419 {
 420         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 421
 422         switch (info->status) {
 423                 case NEW_DATA:
 424                         send_can_msg(orte_data);
 425                         break;
 426                 case DEADLINE:
 427                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 428                         break;
 429         }
 430 }
 431 void rcv_pwr_alert_cb (const ORTERecvInfo *info, void *vinstance,
 432                         void *recvCallBackParam)
 433 {
 434
 435         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 436         switch (info->status) {
 437                 case NEW_DATA:
 438                         set_pwr_alert(orte_data);
 439                         break;
 440                 case DEADLINE:
 441                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 442                         break;
 443         }
 444 }
 445
 446 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 447                                 void *recvCallBackParam)
 448 {
 449         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 450
 451         switch (info->status) {
 452                 case NEW_DATA:
 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");*/
 458                         break;
 459                 case DEADLINE:
 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);
 464                         break;
 465         }
 466 }
 467
 468 void rcv_servos_cb (const ORTERecvInfo *info, void *vinstance,
 469                         void *recvCallBackParam)
 470 {
 471         struct robottype_orte_data *orte_data =
 472                         (struct robottype_orte_data *)recvCallBackParam;
 473
 474         switch (info->status) {
 475                 case NEW_DATA:
 476                         set_serva(orte_data);
 477                         break;
 478                 case DEADLINE:
 479 //                      printf("ORTE deadline occurred - servo receive\n");
 480                         break;
 481         }
 482 }
 483
 484 void rcv_laser_cmd_cb (const ORTERecvInfo *info, void *vinstance,
 485                         void *recvCallBackParam)
 486 {
 487         struct robottype_orte_data *orte_data =
 488                         (struct robottype_orte_data *)recvCallBackParam;
 489
 490         switch (info->status) {
 491                 case NEW_DATA:
 492                         set_laser_cmd(orte_data);
 493                         break;
 494                 case DEADLINE:
 495 //                      printf("ORTE deadline occurred - servo receive\n");
 496                         break;
 497         }
 498 }
 499
 500 void rcv_drives_cb (const ORTERecvInfo *info, void *vinstance,
 501                         void *recvCallBackParam)
 502 {
 503         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 504
 505         switch (info->status) {
 506                 case NEW_DATA:
 507                         set_drives(orte_data);
 508                         break;
 509                 case DEADLINE:
 510 //                      printf("ORTE deadline occurred - drives receive\n");
 511                         break;
 512         }
 513 }
 514
 515 int main(int argc, char *argv[])
 516 {
 517         fd_set read_fd_set;
 518         struct timeval timeout;
 519         int ret;
 520         int size;
 521         int rv = 0;
 522
 523         struct robottype_orte_data orte;
 524         struct can_frame frame;
 525
 526         if (cand_init() < 0) {
 527                 printf("cand: init failed\n");
 528                 exit(1);
 529         } else {
 530                 printf("cand: init OK\n");
 531         }
 532
 533         orte.strength = 1;
 534
 535         /* orte initialization */
 536         robottype_roboorte_init(&orte);
 537
 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);
 548
 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);
 562
 563         robottype_subscriber_drives_create(&orte,
 564                                 rcv_drives_cb, &orte);
 565         /* main loop */
 566         for(;;) {
 567                 FD_ZERO(&read_fd_set);
 568                 FD_SET(sock, &read_fd_set);
 569                 timeout.tv_sec = 0;
 570                 timeout.tv_usec = 10000;
 571
 572                 ret = select(FD_SETSIZE, &read_fd_set, NULL, NULL, &timeout);
 573                 if (ret < 0) {
 574                         perror("cand: select() error");
 575                         goto err;
 576                 }
 577
 578                 /* timeout expired, nothing to be done */
 579                 if (ret == 0)
 580                         continue;
 581
 582                 if (!FD_ISSET(sock, &read_fd_set))
 583                         continue;
 584
 585                 /* read data from SocketCAN */
 586                 size = read(sock, &frame, sizeof(struct can_frame));
 587
 588                 /* parse data */
 589                 cand_parse_frame(&orte, frame);
 590         }
 591
 592 err:
 593         close(sock);
 594         return 1;
 595 out:
 596         return EXIT_SUCCESS;
 597 }