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_robottype.h>
  32 #include <can_msg_masks.h>
  33 #include "cand.h"
  34
  35 int cand_init()
  36 {
  37         if ((sock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
  38                 perror("cand: unable to create a socket");
  39                 return -1;
  40         }
  41
  42         can_addr.can_family = AF_CAN;
  43         std::strcpy(can_ifreq.ifr_name, "can0");
  44
  45         if (ioctl(sock, SIOCGIFINDEX, &can_ifreq) < 0) {
  46                 perror("cand: SIOCGIFINDEX");
  47                 return -2;
  48         }
  49
  50         can_addr.can_ifindex = can_ifreq.ifr_ifindex;
  51
  52         if (!can_addr.can_ifindex) {
  53                 perror("cand: invalid socket interface");
  54                 return -3;
  55         }
  56
  57         if (bind(sock, (struct sockaddr *)&can_addr, sizeof(can_addr)) < 0) {
  58                 perror("cand: unable to bind");
  59                 return -4;
  60         }
  61
  62         return 0;
  63 }
  64
  65 int set_pwr_ctrl(struct robottype_orte_data *orte_data)
  66 {
  67         unsigned char data=0;
  68         if(orte_data->pwr_ctrl.voltage33)
  69                 data |= CAN_PWR_CTRL_33_ON;
  70         else
  71                 data |= CAN_PWR_CTRL_33_OFF;
  72
  73         if(orte_data->pwr_ctrl.voltage50)
  74                 data |= CAN_PWR_CTRL_50_ON;
  75         else
  76                 data |= CAN_PWR_CTRL_50_OFF;
  77
  78         if(orte_data->pwr_ctrl.voltage80)
  79                 data |= CAN_PWR_CTRL_80_ON;
  80         else
  81                 data |= CAN_PWR_CTRL_80_OFF;
  82
  83         can_send(CAN_PWR, 1, &data);
  84
  85         return 0;
  86 }
  87
  88 int set_pwr_alert(struct robottype_orte_data *orte_data)
  89 {
  90         unsigned char data=0;
  91         data = orte_data->pwr_alert.value;
  92
  93         can_send(CAN_PWR_ALERT, 1, &data);
  94
  95         return 0;
  96 }
  97
  98 int send_can_msg(struct robottype_orte_data *orte_data)
  99 {
 100         return can_send(orte_data->can_msg.id, orte_data->can_msg.len, orte_data->can_msg.data);
 101 }
 102
 103 int set_motor_speed(struct robottype_orte_data *orte_data)
 104 {
 105         unsigned char data[4];
 106
 107         data[0] = orte_data->motion_speed.right >> 8;
 108         data[1] = orte_data->motion_speed.right & 0xff;
 109         data[2] = orte_data->motion_speed.left >> 8;
 110         data[3] = orte_data->motion_speed.left & 0xff;
 111         can_send(CAN_MOTION_CMD, 4, data);
 112
 113         return 0;
 114 }
 115
 116 /**
 117  * Sends #CAN_HOKUYO_PITCH message.
 118  *
 119  * - data = orte_data->pusher.pos
 120  */
 121 int set_hokuyo_pitch(struct robottype_orte_data *orte_data)
 122 {
 123         unsigned char data = orte_data->hokuyo_pitch.angle;
 124
 125         can_send(CAN_HOKUYO_PITCH, sizeof(data), &data);
 126         return 0;
 127 }
 128
 129
 130 int can_send(canid_t id, unsigned char length, unsigned char *data)
 131 {
 132         struct can_frame frame;
 133         size_t size;
 134
 135         frame.can_id = id;
 136         frame.can_dlc = length;
 137
 138         for(int i=0; i<length; i++)
 139                 frame.data[i] = data[i];
 140
 141         if ((size = write(sock, &frame, sizeof(struct can_frame))) < 0) {
 142                 perror("cand: can send: write()");
 143                 return -1;
 144         } else if (size < sizeof(struct can_frame)) {
 145                 perror("cand: can send: incomplete CAN frame\n");
 146                 return 1;
 147         }
 148
 149         return 0;
 150 }
 151
 152 /**
 153  * Parse frame ID and react as required
 154  */
 155 void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
 156 {
 157         int status_cnt = 0;
 158
 159         switch(frame.can_id) {
 160                 /* voltage measurements from power board */
 161
 162                 /* robot commands (start, ..) */
 163                 case CAN_ROBOT_CMD:
 164                         orte->robot_cmd.start_condition = frame.data[0];
 165                         ORTEPublicationSend(orte->publication_robot_cmd);
 166                         break;
 167
 168                 /* positioning by odometry */
 169                 case CAN_ODO_DATA:
 170                         orte->odo_data.left =
 171                                         ((frame.data[0]<<24)|
 172                                          (frame.data[1]<<16)|
 173                                          (frame.data[2]<<8));
 174                         orte->odo_data.right =
 175                                         ((frame.data[3]<<24)|
 176                                          (frame.data[4]<<16)|
 177                                          (frame.data[5]<<8));
 178                         ORTEPublicationSend(orte->publication_odo_data);
 179                         break;
 180
 181                 /* positioning by odometry */
 182                 case CAN_MOTION_ODOMETRY_SIMPLE:
 183                         orte->motion_irc.right =
 184                                         ((frame.data[0]<<24)|
 185                                          (frame.data[1]<<16)|
 186                                          (frame.data[2]<<8));
 187                         orte->motion_irc.left =
 188                                         ((frame.data[3]<<24)|
 189                                          (frame.data[4]<<16)|
 190                                          (frame.data[5]<<8));
 191                         orte->motion_irc.seq = frame.data[6];
 192                         ORTEPublicationSend(orte->publication_motion_irc);
 193                         break;
 194
 195                 /* motion status */
 196                 case CAN_MOTION_STATUS:
 197                         orte->motion_status.err_left =
 198                                         (frame.data[0]<<8)|(frame.data[1]);
 199                         orte->motion_status.err_right =
 200                                         (frame.data[2]<<8)|(frame.data[3]);
 201                         if(++status_cnt == 5) {
 202                                 ORTEPublicationSend(orte->publication_motion_status);
 203                                 status_cnt = 0;
 204                         }
 205                         break;
 206                 case CAN_PWR_ADC1:
 207                         double volt33, voltBAT;
 208                         voltBAT = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 209                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 210                         volt33 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 211                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 212                         orte->pwr_voltage.voltage33 = volt33;
 213                         orte->pwr_voltage.voltageBAT = voltBAT;
 214                         break;
 215
 216                 case CAN_PWR_ADC2:
 217                         double volt80, volt50;
 218                         volt50 = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 219                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 220                         volt80 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 221                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 222                         orte->pwr_voltage.voltage80 = volt80;
 223                         orte->pwr_voltage.voltage50 = volt50;
 224
 225                         ORTEPublicationSend(orte->publication_pwr_voltage);
 226
 227                         break;
 228                 default:
 229                         //FIXME: add logging here (Filip)
 230 //                      printf("received CAN msg with unknown id: %x\n",frame.can_id);
 231                         break;
 232         }
 233 }
 234
 235 void rcv_pwr_ctrl_cb (const ORTERecvInfo *info, void *vinstance,
 236                         void *recvCallBackParam)
 237 {
 238         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 239
 240         switch (info->status) {
 241                 case NEW_DATA:
 242                         set_pwr_ctrl(orte_data);
 243                         break;
 244                 case DEADLINE:
 245                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 246                         break;
 247         }
 248 }
 249
 250 void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
 251                         void *recvCallBackParam)
 252 {
 253         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 254
 255         switch (info->status) {
 256                 case NEW_DATA:
 257                         send_can_msg(orte_data);
 258                         break;
 259                 case DEADLINE:
 260                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 261                         break;
 262         }
 263 }
 264 void rcv_pwr_alert_cb (const ORTERecvInfo *info, void *vinstance,
 265                         void *recvCallBackParam)
 266 {
 267
 268         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 269         switch (info->status) {
 270                 case NEW_DATA:
 271                         set_pwr_alert(orte_data);
 272                         break;
 273                 case DEADLINE:
 274                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 275                         break;
 276         }
 277 }
 278
 279 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 280                                 void *recvCallBackParam)
 281 {
 282         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 283
 284         switch (info->status) {
 285                 case NEW_DATA:
 286                         /* reversing motion direction, as it is different, than last year */
 287                         orte_data->motion_speed.left *= -1;
 288                         orte_data->motion_speed.right *=-1;
 289                         set_motor_speed(orte_data);
 290                         /*printf("motor cmd received\n");*/
 291                         break;
 292                 case DEADLINE:
 293                         //printf("motor cmd deadline occurred, stopping motors\n");
 294                         orte_data->motion_speed.left = 0;
 295                         orte_data->motion_speed.right = 0;
 296                         set_motor_speed(orte_data);
 297                         break;
 298         }
 299 }
 300
 301 void rcv_hokuyo_pitch_cb (const ORTERecvInfo *info, void *vinstance,
 302                         void *recvCallBackParam)
 303 {
 304         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 305
 306         switch (info->status) {
 307                 case NEW_DATA:
 308                         set_hokuyo_pitch(orte_data);
 309                         break;
 310                 case DEADLINE:
 311 //                      printf("ORTE deadline occurred - hokuyo pitch receive\n");
 312                         break;
 313         }
 314 }
 315
 316
 317 int main(int argc, char *argv[])
 318 {
 319         fd_set read_fd_set;
 320         struct timeval timeout;
 321         int ret;
 322         int size;
 323
 324         struct robottype_orte_data orte;
 325         struct can_frame frame;
 326
 327         if (cand_init() < 0) {
 328                 printf("cand: init failed\n");
 329                 exit(1);
 330         } else {
 331                 printf("cand: init OK\n");
 332         }
 333
 334         orte.strength = 1;
 335
 336         /* orte initialization */
 337         if(robottype_roboorte_init(&orte)) {
 338                 printf("Roboorte initialization failed! Exiting...\n");
 339                 exit(-1);
 340         }
 341         else
 342                 printf("Roboorte OK\n");
 343
 344         /* creating publishers */
 345         robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 346         robottype_publisher_motion_status_create(&orte, NULL, NULL);
 347         robottype_publisher_odo_data_create(&orte, NULL, NULL);
 348         robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 349         robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 350         robottype_publisher_actuator_status_create(&orte, NULL, NULL);
 351         printf("Publishers OK\n");
 352
 353         /* creating subscribers */
 354         robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_cb, &orte);
 355         robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
 356         robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
 357         robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
 358         robottype_subscriber_hokuyo_pitch_create(&orte, rcv_hokuyo_pitch_cb, &orte);
 359
 360         printf("subscribers OK\n");
 361
 362         /* main loop */
 363         for(;;) {
 364                 FD_ZERO(&read_fd_set);
 365                 FD_SET(sock, &read_fd_set);
 366                 timeout.tv_sec = 0;
 367                 timeout.tv_usec = 10000;
 368
 369                 ret = select(FD_SETSIZE, &read_fd_set, NULL, NULL, &timeout);
 370                 if (ret < 0) {
 371                         perror("cand: select() error");
 372                         goto err;
 373                 }
 374
 375                 /* timeout expired, nothing to be done */
 376                 if (ret == 0)
 377                         continue;
 378
 379                 if (!FD_ISSET(sock, &read_fd_set))
 380                         continue;
 381
 382                 /* read data from SocketCAN */
 383                 size = read(sock, &frame, sizeof(struct can_frame));
 384
 385                 /* parse data */
 386                 cand_parse_frame(&orte, frame);
 387         }
 388         return EXIT_SUCCESS;
 389
 390 err:
 391         close(sock);
 392         return 1;
 393 }