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         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         int 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 int 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 = frame.data[0];
 165                         orte->robot_cmd.stop = frame.data[1];
 166                         ORTEPublicationSend(orte->publication_robot_cmd);
 167                         break;
 168
 169                 /* global sampling period -- time trigger */
 170                 case CAN_CORR_TRIG:
 171                         orte->corr_trig.seq = frame.data[0];
 172                         ORTEPublicationSend(orte->publication_corr_trig);
 173                         break;
 174
 175                 /* distances measured by ULoPoS */
 176                 case CAN_CORR_DIST:
 177                         orte->corr_distances.t1 = (frame.data[1]<<8)|(frame.data[0]);
 178                         orte->corr_distances.t2 = (frame.data[3]<<8)|(frame.data[2]);
 179                         orte->corr_distances.t3 = (frame.data[5]<<8)|(frame.data[4]);
 180                         ORTEPublicationSend(orte->publication_corr_distances);
 181                         break;
 182
 183                 /* positioning by odometry */
 184                 case CAN_ODO_DATA:
 185                         orte->odo_data.left =
 186                                         ((frame.data[0]<<24)|
 187                                          (frame.data[1]<<16)|
 188                                          (frame.data[2]<<8));
 189                         orte->odo_data.right =
 190                                         ((frame.data[3]<<24)|
 191                                          (frame.data[4]<<16)|
 192                                          (frame.data[5]<<8));
 193                         ORTEPublicationSend(orte->publication_odo_data);
 194                         break;
 195
 196                 /* positioning by odometry */
 197                 case CAN_MOTION_ODOMETRY_SIMPLE:
 198                         orte->motion_irc.right =
 199                                         ((frame.data[0]<<24)|
 200                                          (frame.data[1]<<16)|
 201                                          (frame.data[2]<<8));
 202                         orte->motion_irc.left =
 203                                         ((frame.data[3]<<24)|
 204                                          (frame.data[4]<<16)|
 205                                          (frame.data[5]<<8));
 206                         orte->motion_irc.seq = frame.data[6];
 207                         ORTEPublicationSend(orte->publication_motion_irc);
 208                         break;
 209
 210                 /* motion status */
 211                 case CAN_MOTION_STATUS:
 212                         orte->motion_status.err_left =
 213                                         (frame.data[0]<<8)|(frame.data[1]);
 214                         orte->motion_status.err_right =
 215                                         (frame.data[2]<<8)|(frame.data[3]);
 216                         if(++status_cnt == 5) {
 217                                 ORTEPublicationSend(orte->publication_motion_status);
 218                                 status_cnt = 0;
 219                         }
 220                         break;
 221                 case CAN_PWR_ADC1:
 222                         double volt33, voltBAT;
 223                         voltBAT = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 224                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 225                         volt33 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 226                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 227                         orte->pwr_voltage.voltage33 = volt33;
 228                         orte->pwr_voltage.voltageBAT = voltBAT;
 229                         break;
 230
 231                 case CAN_PWR_ADC2:
 232                         double volt80, volt50;
 233                         volt50 = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 234                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 235                         volt80 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 236                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 237                         orte->pwr_voltage.voltage80 = volt80;
 238                         orte->pwr_voltage.voltage50 = volt50;
 239
 240                         ORTEPublicationSend(orte->publication_pwr_voltage);
 241
 242                         break;
 243                 default:
 244                         //FIXME: add logging here (Filip)
 245 //                      printf("received CAN msg with unknown id: %x\n",frame.can_id);
 246                         break;
 247         }
 248 }
 249
 250 void rcv_pwr_ctrl_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                         set_pwr_ctrl(orte_data);
 258                         break;
 259                 case DEADLINE:
 260                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 261                         break;
 262         }
 263 }
 264
 265 void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
 266                         void *recvCallBackParam)
 267 {
 268         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 269
 270         switch (info->status) {
 271                 case NEW_DATA:
 272                         send_can_msg(orte_data);
 273                         break;
 274                 case DEADLINE:
 275                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 276                         break;
 277         }
 278 }
 279 void rcv_pwr_alert_cb (const ORTERecvInfo *info, void *vinstance,
 280                         void *recvCallBackParam)
 281 {
 282
 283         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 284         switch (info->status) {
 285                 case NEW_DATA:
 286                         set_pwr_alert(orte_data);
 287                         break;
 288                 case DEADLINE:
 289                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 290                         break;
 291         }
 292 }
 293
 294 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 295                                 void *recvCallBackParam)
 296 {
 297         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 298
 299         switch (info->status) {
 300                 case NEW_DATA:
 301                         /* reversing motion direction, as it is different, than last year */
 302                         orte_data->motion_speed.left *= -1;
 303                         orte_data->motion_speed.right *=-1;
 304                         set_motor_speed(orte_data);
 305                         /*printf("motor cmd received\n");*/
 306                         break;
 307                 case DEADLINE:
 308                         //printf("motor cmd deadline occurred, stopping motors\n");
 309                         orte_data->motion_speed.left = 0;
 310                         orte_data->motion_speed.right = 0;
 311                         set_motor_speed(orte_data);
 312                         break;
 313         }
 314 }
 315
 316 void rcv_hokuyo_pitch_cb (const ORTERecvInfo *info, void *vinstance,
 317                         void *recvCallBackParam)
 318 {
 319         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 320
 321         switch (info->status) {
 322                 case NEW_DATA:
 323                         set_hokuyo_pitch(orte_data);
 324                         break;
 325                 case DEADLINE:
 326 //                      printf("ORTE deadline occurred - hokuyo pitch receive\n");
 327                         break;
 328         }
 329 }
 330
 331
 332 int main(int argc, char *argv[])
 333 {
 334         fd_set read_fd_set;
 335         struct timeval timeout;
 336         int ret;
 337         int size;
 338
 339         struct robottype_orte_data orte;
 340         struct can_frame frame;
 341
 342         if (cand_init() < 0) {
 343                 printf("cand: init failed\n");
 344                 exit(1);
 345         } else {
 346                 printf("cand: init OK\n");
 347         }
 348
 349         orte.strength = 1;
 350
 351         /* orte initialization */
 352         if(robottype_roboorte_init(&orte)) {
 353                 printf("Roboorte initialization failed! Exiting...\n");
 354                 exit(-1);
 355         }
 356         else
 357                 printf("Roboorte OK\n");
 358
 359         /* creating publishers */
 360         robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 361         robottype_publisher_motion_status_create(&orte, NULL, NULL);
 362         robottype_publisher_odo_data_create(&orte, NULL, NULL);
 363         robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 364         robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 365         robottype_publisher_corr_trig_create(&orte, NULL, NULL);
 366         robottype_publisher_corr_distances_create(&orte, NULL, NULL);
 367         robottype_publisher_actuator_status_create(&orte, NULL, NULL);
 368         printf("Publishers OK\n");
 369
 370         /* creating subscribers */
 371         robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_cb, &orte);
 372         robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
 373         robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
 374         robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
 375         robottype_subscriber_hokuyo_pitch_create(&orte, rcv_hokuyo_pitch_cb, &orte);
 376
 377         printf("subscribers OK\n");
 378
 379         /* main loop */
 380         for(;;) {
 381                 FD_ZERO(&read_fd_set);
 382                 FD_SET(sock, &read_fd_set);
 383                 timeout.tv_sec = 0;
 384                 timeout.tv_usec = 10000;
 385
 386                 ret = select(FD_SETSIZE, &read_fd_set, NULL, NULL, &timeout);
 387                 if (ret < 0) {
 388                         perror("cand: select() error");
 389                         goto err;
 390                 }
 391
 392                 /* timeout expired, nothing to be done */
 393                 if (ret == 0)
 394                         continue;
 395
 396                 if (!FD_ISSET(sock, &read_fd_set))
 397                         continue;
 398
 399                 /* read data from SocketCAN */
 400                 size = read(sock, &frame, sizeof(struct can_frame));
 401
 402                 /* parse data */
 403                 cand_parse_frame(&orte, frame);
 404         }
 405
 406 err:
 407         close(sock);
 408         return 1;
 409 out:
 410         return EXIT_SUCCESS;
 411 }