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