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         return 0;
 126 }
 127
 128 /**
 129  * Sends #CAN_LIFT message.
 130  *
 131  * - data[1] = orte_data->lift.pos & 0xFF; // lower byte
 132  * - data[0] = orte_data->lift.pos >> 8; // upper byte
 133  */
 134 int set_lift(struct robottype_orte_data *orte_data)
 135 {
 136         unsigned char data[2];
 137
 138         data[0] = orte_data->lift.pos >> 8;
 139         data[1] = orte_data->lift.pos & 0xFF;
 140         can_send(CAN_LIFT, sizeof(data), data);
 141
 142         return 0;
 143 }
 144
 145 /**
 146  * FIXME: this function is temporary, just for time LPCs are not ready
 147  * Sends #CAN_PICKER message.
 148  *
 149  * - data[0] = orte_data->picker.rightchela;
 150  * - data[1] = orte_data->picker.leftchela;
 151  * - data[2] = orte_data->picker.leftbelt;
 152  * - data[3] = orte_data->picker.rightbelt;
 153  * - data[4] = orte_data->picker.timeout;
 154  */
 155 int set_picker(struct robottype_orte_data *orte_data)
 156 {
 157         unsigned char data[5];
 158
 159         data[0] = orte_data->picker.rightchela;
 160         data[1] = orte_data->picker.leftchela;
 161         data[2] = orte_data->picker.leftbelt;
 162         data[3] = orte_data->picker.rightbelt;
 163         data[4] = orte_data->picker.timeout;
 164         can_send(CAN_PICKER, sizeof(data), data);
 165
 166         return 0;
 167 }
 168
 169 /**
 170  * Sends #CAN_SERVO message.
 171  *
 172  * - data[1] = orte_data->chelas.left;
 173  * - data[0] = orte_data->chelas.right;
 174  */
 175 int set_chelas(struct robottype_orte_data *orte_data)
 176 {
 177         unsigned char data[2];
 178
 179         data[0] = orte_data->chelas.left;
 180         data[1] = orte_data->chelas.right;
 181         can_send(CAN_SERVO, sizeof(data), data);
 182
 183         return 0;
 184 }
 185
 186 /**
 187  * Sends #CAN_DRIVES message.
 188  *
 189  * - data[1] = orte_data->belts.left;
 190  * - data[0] = orte_data->belts.right;
 191  */
 192 int set_belts(struct robottype_orte_data *orte_data)
 193 {
 194         unsigned char data[2];
 195
 196         data[0] = orte_data->belts.left;
 197         data[1] = orte_data->belts.right;
 198         can_send(CAN_DRIVES, sizeof(data), data);
 199
 200         return 0;
 201 }
 202
 203 /**
 204  * Sends #CAN_HOLDER message.
 205  *
 206  * - data[0] = orte_data->holder.pos;
 207  */
 208 int set_holder(struct robottype_orte_data *orte_data)
 209 {
 210         unsigned char data[2];
 211
 212         data[0] = orte_data->holder.pos;
 213         can_send(CAN_HOLDER, sizeof(data), data);
 214
 215         return 0;
 216 }
 217
 218 /**
 219  * Sends #CAN_PUSHER message.
 220  *
 221  * - data[1] = orte_data->pusher.pos & 0xFF; // lower byte
 222  * - data[0] = orte_data->pusher.pos >> 8; // upper byte
 223  */
 224 int set_pusher(struct robottype_orte_data *orte_data)
 225 {
 226         unsigned char data[2];
 227
 228         data[0] = orte_data->pusher.pos >> 8;
 229         data[1] = orte_data->pusher.pos & 0xFF;
 230         can_send(CAN_PUSHER, sizeof(data), data);
 231
 232         return 0;
 233 }
 234
 235
 236 int can_send(canid_t id, unsigned char length, unsigned char *data)
 237 {
 238         struct can_frame frame;
 239         int size;
 240
 241         frame.can_id = id;
 242         frame.can_dlc = length;
 243
 244         for(int i=0; i<length; i++)
 245                 frame.data[i] = data[i];
 246
 247         if ((size = write(sock, &frame, sizeof(struct can_frame))) < 0) {
 248                 perror("cand: can send: write()");
 249                 return -1;
 250         } else if (size < sizeof(struct can_frame)) {
 251                 perror("cand: can send: incomplete CAN frame\n");
 252                 return 1;
 253         }
 254
 255         return 0;
 256 }
 257
 258 /**
 259  * Parse frame ID and react as required
 260  */
 261 int cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
 262 {
 263         int status_cnt = 0;
 264         int adc1_cnt = 0;
 265         int adc2_cnt = 0;
 266         int adc3_cnt = 0;
 267         int ir_cnt = 0;
 268         unsigned short *las_meas;
 269         int las_mi, las_di, las_bcnt;
 270         static unsigned short prev_meas;
 271         int i;
 272
 273         switch(frame.can_id) {
 274                 /* voltage measurements from power board */
 275
 276                 /* robot commands (start, ..) */
 277                 case CAN_ROBOT_CMD:
 278                         orte->robot_cmd.start = frame.data[0];
 279                         orte->robot_cmd.stop = frame.data[1];
 280                         ORTEPublicationSend(orte->publication_robot_cmd);
 281                         break;
 282
 283                 /* global sampling period -- time trigger */
 284                 case CAN_CORR_TRIG:
 285                         orte->corr_trig.seq = frame.data[0];
 286                         ORTEPublicationSend(orte->publication_corr_trig);
 287                         break;
 288
 289                 /* distances measured by ULoPoS */
 290                 case CAN_CORR_DIST:
 291                         orte->corr_distances.t1 = (frame.data[1]<<8)|(frame.data[0]);
 292                         orte->corr_distances.t2 = (frame.data[3]<<8)|(frame.data[2]);
 293                         orte->corr_distances.t3 = (frame.data[5]<<8)|(frame.data[4]);
 294                         ORTEPublicationSend(orte->publication_corr_distances);
 295                         break;
 296
 297                 /* positioning by odometry */
 298                 case CAN_MOTION_ODOMETRY_SIMPLE:
 299                         orte->motion_irc.right =
 300                                         ((frame.data[0]<<24)|(frame.data[1]<<16)|
 301                                         (frame.data[2]<<8)|(frame.data[3]));
 302                         orte->motion_irc.left =
 303                                         ((frame.data[4]<<24)|(frame.data[5]<<16)|
 304                                          (frame.data[6]<<8)|(frame.data[7]));
 305                         ORTEPublicationSend(orte->publication_motion_irc);
 306                         break;
 307
 308                 /* motion status */
 309                 case CAN_MOTION_STATUS:
 310                         orte->motion_status.err_left =
 311                                         (frame.data[0]<<8)|(frame.data[1]);
 312                         orte->motion_status.err_right =
 313                                         (frame.data[2]<<8)|(frame.data[3]);
 314                         if(++status_cnt == 5) {
 315                                 ORTEPublicationSend(orte->publication_motion_status);
 316                                 status_cnt = 0;
 317                         }
 318                         break;
 319                 case CAN_PWR_ADC1:
 320                         double volt33, voltBAT;
 321                         voltBAT = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 322                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 323                         volt33 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 324                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 325                         orte->pwr_voltage.voltage33 = volt33;
 326                         orte->pwr_voltage.voltageBAT = voltBAT;
 327                         break;
 328
 329                 case CAN_PWR_ADC2:
 330                         double volt80, volt50;
 331                         volt50 = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 332                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 333                         volt80 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 334                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 335                         orte->pwr_voltage.voltage80 = volt80;
 336                         orte->pwr_voltage.voltage50 = volt50;
 337
 338                         ORTEPublicationSend(orte->publication_pwr_voltage);
 339
 340                         break;
 341                 case CAN_ADC_1:
 342                 //      ORTEPublicationSend(orte->publication_sharps);
 343                         break;
 344
 345                 /* laser data */
 346                 case CAN_CMU:
 347                         orte->cmu.color = frame.data[0];
 348                         ORTEPublicationSend(orte->publication_cmu);
 349                         break;
 350
 351                 default:
 352 //                      printf("received CAN msg with unknown id: %x\n",frame.can_id);
 353                         break;
 354         }
 355 }
 356
 357 void rcv_pwr_ctrl_cb (const ORTERecvInfo *info, void *vinstance,
 358                         void *recvCallBackParam)
 359 {
 360         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 361
 362         switch (info->status) {
 363                 case NEW_DATA:
 364                         set_pwr_ctrl(orte_data);
 365                         break;
 366                 case DEADLINE:
 367                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 368                         break;
 369         }
 370 }
 371
 372 void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
 373                         void *recvCallBackParam)
 374 {
 375         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 376
 377         switch (info->status) {
 378                 case NEW_DATA:
 379                         send_can_msg(orte_data);
 380                         break;
 381                 case DEADLINE:
 382                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 383                         break;
 384         }
 385 }
 386 void rcv_pwr_alert_cb (const ORTERecvInfo *info, void *vinstance,
 387                         void *recvCallBackParam)
 388 {
 389
 390         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 391         switch (info->status) {
 392                 case NEW_DATA:
 393                         set_pwr_alert(orte_data);
 394                         break;
 395                 case DEADLINE:
 396                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 397                         break;
 398         }
 399 }
 400
 401 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 402                                 void *recvCallBackParam)
 403 {
 404         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 405
 406         switch (info->status) {
 407                 case NEW_DATA:
 408                         /* reversing motion direction, as it is different, than last year */
 409                         orte_data->motion_speed.left *= -1;
 410                         orte_data->motion_speed.right *=-1;
 411                         set_motor_speed(orte_data);
 412                         /*printf("motor cmd received\n");*/
 413                         break;
 414                 case DEADLINE:
 415                         printf("motor cmd deadline occurred, stopping motors\n");
 416                         orte_data->motion_speed.left = 0;
 417                         orte_data->motion_speed.right = 0;
 418                         set_motor_speed(orte_data);
 419                         break;
 420         }
 421 }
 422
 423 void rcv_lift_cb (const ORTERecvInfo *info, void *vinstance,
 424                         void *recvCallBackParam)
 425 {
 426         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 427
 428         switch (info->status) {
 429                 case NEW_DATA:
 430                         set_lift(orte_data);
 431                         break;
 432                 case DEADLINE:
 433 //                      printf("ORTE deadline occurred - lift receive\n");
 434                         break;
 435         }
 436 }
 437
 438 void rcv_picker_cb (const ORTERecvInfo *info, void *vinstance,
 439                         void *recvCallBackParam)
 440 {
 441         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 442
 443         switch (info->status) {
 444                 case NEW_DATA:
 445                         set_picker(orte_data);
 446                         break;
 447                 case DEADLINE:
 448 //                      printf("ORTE deadline occurred - chelas receive\n");
 449                         break;
 450         }
 451 }
 452
 453 void rcv_chelas_cb (const ORTERecvInfo *info, void *vinstance,
 454                         void *recvCallBackParam)
 455 {
 456         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 457
 458         switch (info->status) {
 459                 case NEW_DATA:
 460                         set_chelas(orte_data);
 461                         break;
 462                 case DEADLINE:
 463 //                      printf("ORTE deadline occurred - chelas receive\n");
 464                         break;
 465         }
 466 }
 467
 468 void rcv_belts_cb (const ORTERecvInfo *info, void *vinstance,
 469                         void *recvCallBackParam)
 470 {
 471         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 472
 473         switch (info->status) {
 474                 case NEW_DATA:
 475                         set_belts(orte_data);
 476                         break;
 477                 case DEADLINE:
 478 //                      printf("ORTE deadline occurred - belts receive\n");
 479                         break;
 480         }
 481 }
 482
 483 void rcv_holder_cb (const ORTERecvInfo *info, void *vinstance,
 484                         void *recvCallBackParam)
 485 {
 486         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 487
 488         switch (info->status) {
 489                 case NEW_DATA:
 490                         set_holder(orte_data);
 491                         break;
 492                 case DEADLINE:
 493 //                      printf("ORTE deadline occurred - holder receive\n");
 494                         break;
 495         }
 496 }
 497
 498 void rcv_pusher_cb (const ORTERecvInfo *info, void *vinstance,
 499                         void *recvCallBackParam)
 500 {
 501         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 502
 503         switch (info->status) {
 504                 case NEW_DATA:
 505                         set_pusher(orte_data);
 506                         break;
 507                 case DEADLINE:
 508 //                      printf("ORTE deadline occurred - pusher receive\n");
 509                         break;
 510         }
 511 }
 512
 513 int main(int argc, char *argv[])
 514 {
 515         fd_set read_fd_set;
 516         struct timeval timeout;
 517         int ret;
 518         int size;
 519         int rv = 0;
 520
 521         struct robottype_orte_data orte;
 522         struct can_frame frame;
 523
 524         if (cand_init() < 0) {
 525                 printf("cand: init failed\n");
 526                 exit(1);
 527         } else {
 528                 printf("cand: init OK\n");
 529         }
 530
 531         orte.strength = 1;
 532
 533         /* orte initialization */
 534         if(robottype_roboorte_init(&orte)) {
 535                 printf("Roboorte initialization failed! Exiting...\n");
 536                 exit(-1);
 537         }
 538
 539         /* creating publishers */
 540         robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 541         robottype_publisher_motion_status_create(&orte, NULL, NULL);
 542         robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 543         robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 544         robottype_publisher_cmu_create(&orte, NULL, NULL);
 545
 546         /* creating subscribers */
 547         robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_cb, &orte);
 548         robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
 549         robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
 550         robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
 551         robottype_subscriber_lift_create(&orte, rcv_lift_cb, &orte);
 552         robottype_subscriber_chelas_create(&orte, rcv_chelas_cb, &orte);
 553         robottype_subscriber_belts_create(&orte, rcv_belts_cb, &orte);
 554         robottype_subscriber_picker_create(&orte, rcv_picker_cb, &orte); // FIXME: temporary (LPCs not ready)
 555         robottype_subscriber_holder_create(&orte, rcv_holder_cb, &orte);
 556         robottype_subscriber_pusher_create(&orte, rcv_pusher_cb, &orte);
 557
 558         /* main loop */
 559         for(;;) {
 560                 FD_ZERO(&read_fd_set);
 561                 FD_SET(sock, &read_fd_set);
 562                 timeout.tv_sec = 0;
 563                 timeout.tv_usec = 10000;
 564
 565                 ret = select(FD_SETSIZE, &read_fd_set, NULL, NULL, &timeout);
 566                 if (ret < 0) {
 567                         perror("cand: select() error");
 568                         goto err;
 569                 }
 570
 571                 /* timeout expired, nothing to be done */
 572                 if (ret == 0)
 573                         continue;
 574
 575                 if (!FD_ISSET(sock, &read_fd_set))
 576                         continue;
 577
 578                 /* read data from SocketCAN */
 579                 size = read(sock, &frame, sizeof(struct can_frame));
 580
 581                 /* parse data */
 582                 cand_parse_frame(&orte, frame);
 583         }
 584
 585 err:
 586         close(sock);
 587         return 1;
 588 out:
 589         return EXIT_SUCCESS;
 590 }