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 #include <poll.h>
  23
  24 #include <af_can.h>
  25 #include <canutils.h>
  26
  27 #include <can_ids.h>
  28 #include <can_msg_masks.h>
  29 #include <can_msg_def.h>
  30 #include <sharp.h>
  31 #include <orte.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         return can_send(orte_data->can_msg.id, orte_data->can_msg.len, orte_data->can_msg.data);
 102 }
 103
 104 int set_motor_speed(struct robottype_orte_data *orte_data)
 105 {
 106         unsigned char data[4];
 107
 108         data[0] = orte_data->motion_speed.right >> 8;
 109         data[1] = orte_data->motion_speed.right & 0xff;
 110         data[2] = orte_data->motion_speed.left >> 8;
 111         data[3] = orte_data->motion_speed.left & 0xff;
 112         can_send(CAN_MOTION_CMD, 4, data);
 113
 114         return 0;
 115 }
 116
 117 int set_vidle_cmd(uint16_t req_pos)
 118 {
 119         unsigned char data[2];
 120
 121         data[0] = req_pos >> 8;
 122         data[1] = req_pos & 0xff;
 123         can_send(CAN_VIDLE_CMD, 2, data);
 124
 125         return 0;
 126 }
 127
 128 /**
 129  * Sends #CAN_HOKUYO_PITCH message.
 130  *
 131  * - data = orte_data->pusher.pos
 132  */
 133 int set_hokuyo_pitch(struct robottype_orte_data *orte_data)
 134 {
 135         unsigned char data = orte_data->hokuyo_pitch.angle;
 136
 137         can_send(CAN_HOKUYO_PITCH, sizeof(data), &data);
 138         return 0;
 139 }
 140
 141
 142 int can_send(canid_t id, unsigned char length, unsigned char *data)
 143 {
 144         struct can_frame frame;
 145         size_t size;
 146
 147         frame.can_id = id;
 148         frame.can_dlc = length;
 149
 150         for(int i=0; i<length; i++)
 151                 frame.data[i] = data[i];
 152
 153         if ((size = write(sock, &frame, sizeof(struct can_frame))) < 0) {
 154                 perror("cand: can send: write()");
 155                 return -1;
 156         } else if (size < sizeof(struct can_frame)) {
 157                 perror("cand: can send: incomplete CAN frame\n");
 158                 return 1;
 159         }
 160
 161         return 0;
 162 }
 163
 164 /**
 165  * Parse frame ID and react as required
 166  */
 167 void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
 168 {
 169         static int status_cnt = 0;
 170
 171         switch(frame.can_id) {
 172                 /* voltage measurements from power board */
 173
 174                 /* robot commands (start, ..) */
 175                 case CAN_VIDLE_STATUS:
 176                         orte->vidle_status.act_pos = (frame.data[0] << 8) | frame.data[1];
 177                         orte->vidle_status.response = (frame.data[2] << 8) | frame.data[3];
 178                         orte->vidle_status.flags = frame.data[4];
 179                         ORTEPublicationSend(orte->publication_vidle_status);
 180                         break;
 181                 case CAN_ROBOT_CMD:
 182                         orte->robot_cmd.start_condition = frame.data[0];
 183                         ORTEPublicationSend(orte->publication_robot_cmd);
 184                         break;
 185                 case CAN_ROBOT_SWITCHES:
 186                         orte->robot_switches.bumper_pressed = !!(frame.data[0] & CAN_SWITCH_BUMPER);
 187                         orte->robot_switches.bumper_left = !!(frame.data[0] & CAN_SWITCH_LEFT);
 188                         orte->robot_switches.bumper_right = !!(frame.data[0] & CAN_SWITCH_RIGHT);
 189                         orte->robot_switches.team_color = (frame.data[0] & CAN_SWITCH_COLOR) ? 1 : 0;
 190                         ORTEPublicationSend(orte->publication_robot_switches);
 191                         break;
 192
 193                 /* positioning by odometry */
 194                 case CAN_ODO_DATA:
 195                         orte->odo_data.left =
 196                                         ((frame.data[0]<<24)|
 197                                          (frame.data[1]<<16)|
 198                                          (frame.data[2]<<8));
 199                         orte->odo_data.right =
 200                                         ((frame.data[3]<<24)|
 201                                          (frame.data[4]<<16)|
 202                                          (frame.data[5]<<8));
 203                         ORTEPublicationSend(orte->publication_odo_data);
 204                         break;
 205
 206                 /* positioning by odometry */
 207                 case CAN_MOTION_ODOMETRY_SIMPLE:
 208                         orte->motion_irc.right =
 209                                         ((frame.data[0]<<24)|
 210                                          (frame.data[1]<<16)|
 211                                          (frame.data[2]<<8));
 212                         orte->motion_irc.left =
 213                                         ((frame.data[3]<<24)|
 214                                          (frame.data[4]<<16)|
 215                                          (frame.data[5]<<8));
 216                         orte->motion_irc.seq = frame.data[6];
 217                         ORTEPublicationSend(orte->publication_motion_irc);
 218                         break;
 219
 220                 /* motion status */
 221                 case CAN_MOTION_STATUS:
 222                         orte->motion_status.err_left =
 223                                         (frame.data[0]<<8)|(frame.data[1]);
 224                         orte->motion_status.err_right =
 225                                         (frame.data[2]<<8)|(frame.data[3]);
 226                         if(++status_cnt == 2) { // h8eurobot sends duplicite can messages
 227                                 ORTEPublicationSend(orte->publication_motion_status);
 228                                 status_cnt = 0;
 229                         }
 230                         break;
 231                 case CAN_PWR_ADC1:
 232                         double volt33, voltBAT;
 233                         voltBAT = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 234                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 235                         volt33 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 236                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 237                         orte->pwr_voltage.voltage33 = volt33;
 238                         orte->pwr_voltage.voltageBAT = voltBAT;
 239                         break;
 240
 241                 case CAN_PWR_ADC2:
 242                         double volt80, volt50;
 243                         volt50 = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 244                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 245                         volt80 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 246                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 247                         orte->pwr_voltage.voltage80 = volt80;
 248                         orte->pwr_voltage.voltage50 = volt50;
 249
 250                         ORTEPublicationSend(orte->publication_pwr_voltage);
 251
 252                         break;
 253                 default:
 254                         //FIXME: add logging here (Filip)
 255 //                      printf("received CAN msg with unknown id: %x\n",frame.can_id);
 256                         break;
 257         }
 258 }
 259
 260 void rcv_pwr_ctrl_cb (const ORTERecvInfo *info, void *vinstance,
 261                         void *recvCallBackParam)
 262 {
 263         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 264
 265         switch (info->status) {
 266                 case NEW_DATA:
 267                         set_pwr_ctrl(orte_data);
 268                         break;
 269                 case DEADLINE:
 270                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 271                         break;
 272         }
 273 }
 274
 275 void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
 276                         void *recvCallBackParam)
 277 {
 278         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 279
 280         switch (info->status) {
 281                 case NEW_DATA:
 282                         send_can_msg(orte_data);
 283                         break;
 284                 case DEADLINE:
 285                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 286                         break;
 287         }
 288 }
 289 void rcv_pwr_alert_cb (const ORTERecvInfo *info, void *vinstance,
 290                         void *recvCallBackParam)
 291 {
 292
 293         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 294         switch (info->status) {
 295                 case NEW_DATA:
 296                         set_pwr_alert(orte_data);
 297                         break;
 298                 case DEADLINE:
 299                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 300                         break;
 301         }
 302 }
 303
 304 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 305                                 void *recvCallBackParam)
 306 {
 307         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 308
 309         switch (info->status) {
 310                 case NEW_DATA:
 311                         /* reversing motion direction, as it is different, than last year */
 312                         orte_data->motion_speed.left *= -1;
 313                         orte_data->motion_speed.right *=-1;
 314                         set_motor_speed(orte_data);
 315                         /*printf("motor cmd received\n");*/
 316                         break;
 317                 case DEADLINE:
 318                         //printf("motor cmd deadline occurred, stopping motors\n");
 319                         orte_data->motion_speed.left = 0;
 320                         orte_data->motion_speed.right = 0;
 321                         set_motor_speed(orte_data);
 322                         break;
 323         }
 324 }
 325
 326 void rcv_vidle_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 327                       void *recvCallBackParam)
 328 {
 329         struct vidle_cmd_type *vidle_cmd = (struct vidle_cmd_type *)vinstance;
 330
 331         switch (info->status) {
 332                 case NEW_DATA:
 333                         set_vidle_cmd(vidle_cmd->req_pos);
 334                         break;
 335                 case DEADLINE:
 336                         break;
 337         }
 338 }
 339
 340 void rcv_hokuyo_pitch_cb (const ORTERecvInfo *info, void *vinstance,
 341                         void *recvCallBackParam)
 342 {
 343         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 344
 345         switch (info->status) {
 346                 case NEW_DATA:
 347                         set_hokuyo_pitch(orte_data);
 348                         break;
 349                 case DEADLINE:
 350 //                      printf("ORTE deadline occurred - hokuyo pitch receive\n");
 351                         break;
 352         }
 353 }
 354
 355
 356 int main(int argc, char *argv[])
 357 {
 358         int ret;
 359         int size;
 360
 361         struct robottype_orte_data orte;
 362         struct can_frame frame;
 363
 364         if (cand_init() < 0) {
 365                 printf("cand: init failed\n");
 366                 exit(1);
 367         } else {
 368                 printf("cand: init OK\n");
 369         }
 370
 371         orte.strength = 1;
 372
 373         /* orte initialization */
 374         if(robottype_roboorte_init(&orte)) {
 375                 printf("Roboorte initialization failed! Exiting...\n");
 376                 exit(-1);
 377         }
 378         else
 379                 printf("Roboorte OK\n");
 380
 381         /* creating publishers */
 382         robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 383         robottype_publisher_motion_status_create(&orte, NULL, NULL);
 384         robottype_publisher_odo_data_create(&orte, NULL, NULL);
 385         robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 386         robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 387         robottype_publisher_robot_switches_create(&orte, NULL, NULL);
 388         robottype_publisher_vidle_status_create(&orte, NULL, NULL);
 389         printf("Publishers OK\n");
 390
 391         /* creating subscribers */
 392         robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_cb, &orte);
 393         robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
 394         robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
 395         robottype_subscriber_vidle_cmd_create(&orte,    rcv_vidle_cmd_cb, &orte);
 396         robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
 397
 398
 399         printf("subscribers OK\n");
 400
 401         /* main loop */
 402         for(;;) {
 403                 struct pollfd pfd[1] = {{sock, POLLIN, 0}};
 404
 405                 ret = poll(pfd, 1, -1);
 406                 if (ret < 0) {
 407                         perror("cand: poll() error");
 408                         goto err;
 409                 }
 410
 411                 /* timeout expired, nothing to be done */
 412                 if (ret == 0)
 413                         continue;
 414
 415                 /* read data from SocketCAN */
 416                 size = read(sock, &frame, sizeof(struct can_frame));
 417
 418                 /* parse data */
 419                 if (size > 0)
 420                         cand_parse_frame(&orte, frame);
 421         }
 422         return EXIT_SUCCESS;
 423
 424 err:
 425         close(sock);
 426         return 1;
 427 }