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 #include <unistd.h>
  24 #include <stdio.h>
  25
  26 #include <af_can.h>
  27 #include <canutils.h>
  28
  29 #include <can_ids.h>
  30 #include <can_msg_masks.h>
  31 #include <can_msg_def.h>
  32 #include <orte.h>
  33 #include <roboorte_robottype.h>
  34 #include <can_msg_masks.h>
  35 #include "cand.h"
  36
  37 int cand_init()
  38 {
  39         if ((sock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
  40                 perror("cand: unable to create a socket");
  41                 return -1;
  42         }
  43
  44         can_addr.can_family = AF_CAN;
  45         std::strcpy(can_ifreq.ifr_name, "can0");
  46
  47         if (ioctl(sock, SIOCGIFINDEX, &can_ifreq) < 0) {
  48                 perror("cand: SIOCGIFINDEX");
  49                 return -2;
  50         }
  51
  52         can_addr.can_ifindex = can_ifreq.ifr_ifindex;
  53
  54         if (!can_addr.can_ifindex) {
  55                 perror("cand: invalid socket interface");
  56                 return -3;
  57         }
  58
  59         if (bind(sock, (struct sockaddr *)&can_addr, sizeof(can_addr)) < 0) {
  60                 perror("cand: unable to bind");
  61                 return -4;
  62         }
  63
  64         return 0;
  65 }
  66
  67 int set_pwr_ctrl(struct robottype_orte_data *orte_data)
  68 {
  69         unsigned char data=0;
  70         if(orte_data->pwr_ctrl.voltage33)
  71                 data |= CAN_PWR_CTRL_33_ON;
  72         else
  73                 data |= CAN_PWR_CTRL_33_OFF;
  74
  75         if(orte_data->pwr_ctrl.voltage50)
  76                 data |= CAN_PWR_CTRL_50_ON;
  77         else
  78                 data |= CAN_PWR_CTRL_50_OFF;
  79
  80         if(orte_data->pwr_ctrl.voltage80)
  81                 data |= CAN_PWR_CTRL_80_ON;
  82         else
  83                 data |= CAN_PWR_CTRL_80_OFF;
  84
  85         can_send(CAN_PWR, 1, &data);
  86
  87         return 0;
  88 }
  89
  90 int send_can_msg(struct robottype_orte_data *orte_data)
  91 {
  92         return can_send(orte_data->can_msg.id, orte_data->can_msg.len, orte_data->can_msg.data);
  93 }
  94
  95 int set_motor_speed(struct robottype_orte_data *orte_data)
  96 {
  97         unsigned char data[4];
  98
  99         data[0] = orte_data->motion_speed.right >> 8;
 100         data[1] = orte_data->motion_speed.right & 0xff;
 101         data[2] = orte_data->motion_speed.left >> 8;
 102         data[3] = orte_data->motion_speed.left & 0xff;
 103         can_send(CAN_MOTION_CMD, 4, data);
 104
 105         return 0;
 106 }
 107
 108 int can_send(canid_t id, unsigned char length, unsigned char *data)
 109 {
 110         struct can_frame frame;
 111         size_t size;
 112
 113         frame.can_id = id;
 114         frame.can_dlc = length;
 115
 116         for(int i=0; i<length; i++)
 117                 frame.data[i] = data[i];
 118
 119         if ((size = write(sock, &frame, sizeof(struct can_frame))) < 0) {
 120                 perror("cand: can send: write()");
 121                 return -1;
 122         } else if (size < sizeof(struct can_frame)) {
 123                 perror("cand: can send: incomplete CAN frame\n");
 124                 return 1;
 125         }
 126
 127         return 0;
 128 }
 129
 130 /**
 131  * Parse frame ID and react as required
 132  */
 133 void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
 134 {
 135         static int status_cnt = 0;
 136
 137         switch(frame.can_id) {
 138                 /* robot commands (start, ..) */
 139                 case CAN_CL_SENSOR_STATUS:
 140                         orte->cl_sensor_status.pattern_match = frame.data[0];
 141                         ORTEPublicationSend(orte->publication_cl_sensor_status);
 142                         break;
 143                 case CAN_ROBOT_CMD:
 144                         orte->robot_cmd.start_condition = frame.data[0];
 145                         ORTEPublicationSend(orte->publication_robot_cmd);
 146                         break;
 147                 case CAN_ROBOT_SWITCHES:
 148                         /*
 149                         char tmp_color = 0;
 150                         if (frame.data[0] & CAN_SWITCH_COLOR_0) tmp_color += 1;
 151                         if (frame.data[0] & CAN_SWITCH_COLOR_1) tmp_color += 2;
 152                         orte->robot_switches.team_color = tmp_color;
 153                         */
 154                         orte->robot_switches.team_color = frame.data[0] & (CAN_SWITCH_COLOR_0 + CAN_SWITCH_COLOR_1);
 155                         orte->robot_switches.strategy = (frame.data[0] & CAN_SWITCH_STRATEGY ? 1 : 0);
 156                         orte->robot_switches.home = (frame.data[0] & CAN_SWITCH_HOME ? 1 : 0);
 157                         ORTEPublicationSend(orte->publication_robot_switches);
 158                         break;
 159                 case CAN_ROBOT_BUMPERS:
 160                         orte->robot_bumpers.bumper_left = (frame.data[0] & CAN_BUMPER_LEFT) ? 0 : 1;
 161                         orte->robot_bumpers.bumper_left_across = (frame.data[0] & CAN_BUMPER_LEFT_ACROSS) ? 0 : 1;
 162                         orte->robot_bumpers.bumper_right = (frame.data[0] & CAN_BUMPER_RIGHT) ? 0 : 1;
 163                         orte->robot_bumpers.bumper_right_across = (frame.data[0] & CAN_BUMPER_RIGHT_ACROSS) ? 0 : 1;
 164                         orte->robot_bumpers.bumper_rear_left = (frame.data[0] & CAN_BUMPER_REAR_LEFT) ? 0 : 1;
 165                         orte->robot_bumpers.bumper_rear_right = (frame.data[0] & CAN_BUMPER_REAR_RIGHT) ? 0 : 1;
 166                         ORTEPublicationSend(orte->publication_robot_bumpers);
 167                 break;
 168
 169                 /* positioning by independent odometry */
 170                 case CAN_ODO_DATA:
 171                         orte->odo_data.right =
 172                                         ((frame.data[0]<<24)|
 173                                          (frame.data[1]<<16)|
 174                                          (frame.data[2]<<8));
 175                         orte->odo_data.left =
 176                                         ((frame.data[3]<<24)|
 177                                          (frame.data[4]<<16)|
 178                                          (frame.data[5]<<8));
 179                         ORTEPublicationSend(orte->publication_odo_data);
 180                         break;
 181
 182                 /* positioning by odometry from motors*/
 183                 case CAN_MOTION_ODOMETRY_SIMPLE:
 184                         orte->motion_irc.left =
 185                                         ((frame.data[0]<<24)|
 186                                          (frame.data[1]<<16)|
 187                                          (frame.data[2]<<8));
 188                         orte->motion_irc.right =
 189                                         ((frame.data[3]<<24)|
 190                                          (frame.data[4]<<16)|
 191                                          (frame.data[5]<<8));
 192                         orte->motion_irc.seq = frame.data[6];
 193                         ORTEPublicationSend(orte->publication_motion_irc);
 194                         break;
 195
 196                 /* motion status */
 197                 case CAN_MOTION_STATUS:
 198                         orte->motion_status.err_left =
 199                                         (frame.data[0]<<8)|(frame.data[1]);
 200                         orte->motion_status.err_right =
 201                                         (frame.data[2]<<8)|(frame.data[3]);
 202                         if(++status_cnt == 2) { // h8eurobot sends duplicite can messages
 203                                 ORTEPublicationSend(orte->publication_motion_status);
 204                                 status_cnt = 0;
 205                         }
 206                         break;
 207                 /* voltage measurements from power board */
 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
 231                 case CAN_PWR_ALERT:
 232                         orte->pwr_alert.alert_33 = (frame.data[0] & CAN_PWR_ALERT_33 ? 1 : 0);
 233                         orte->pwr_alert.alert_50 = (frame.data[0] & CAN_PWR_ALERT_50 ? 1 : 0);
 234                         orte->pwr_alert.alert_80 = (frame.data[0] & CAN_PWR_ALERT_80 ? 1 : 0);
 235                         orte->pwr_alert.bat_full = (frame.data[0] & CAN_PWR_BATT_FULL ? 1 : 0);
 236                         orte->pwr_alert.bat_mean = (frame.data[0] & CAN_PWR_BATT_MEAN ? 1 : 0);
 237                         orte->pwr_alert.bat_low = (frame.data[0] & CAN_PWR_BATT_LOW ? 1 : 0);
 238                         orte->pwr_alert.bat_critical = (frame.data[0] & CAN_PWR_BATT_CRITICAL ? 1 : 0);
 239
 240                         ORTEPublicationSend(orte->publication_pwr_alert);
 241                         break;
 242
 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
 280 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 281                                 void *recvCallBackParam)
 282 {
 283         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 284
 285         switch (info->status) {
 286                 case NEW_DATA:
 287                         /* reversing motion direction, as it is different, than last year */
 288                         orte_data->motion_speed.left *= 1;
 289                         orte_data->motion_speed.right *= 1;
 290                         set_motor_speed(orte_data);
 291                         /*printf("motor cmd received\n");*/
 292                         break;
 293                 case DEADLINE:
 294                         //printf("motor cmd deadline occurred, stopping motors\n");
 295                         orte_data->motion_speed.left = 0;
 296                         orte_data->motion_speed.right = 0;
 297                         set_motor_speed(orte_data);
 298                         break;
 299         }
 300 }
 301
 302 void rcv_cl_sensor_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 303                       void *recvCallBackParam)
 304 {
 305         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 306
 307         switch (info->status) {
 308                 case NEW_DATA:
 309                         unsigned char data[1];
 310                         data[0] = orte_data->cl_sensor_cmd.bank_nbr;
 311                         can_send(CAN_CL_SENSOR_CMD, 1, data);
 312                         break;
 313                 case DEADLINE:
 314                         break;
 315         }
 316 }
 317
 318 int main(int argc, char *argv[])
 319 {
 320         int ret;
 321         int size;
 322
 323         struct robottype_orte_data orte;
 324         struct can_frame frame;
 325
 326         if (cand_init() < 0) {
 327                 printf("cand: init failed\n");
 328                 exit(1);
 329         } else {
 330                 printf("cand: init OK\n");
 331         }
 332
 333         orte.strength = 1;
 334
 335         /* orte initialization */
 336         if(robottype_roboorte_init(&orte)) {
 337                 printf("Roboorte initialization failed! Exiting...\n");
 338                 exit(-1);
 339         }
 340         else
 341                 printf("Roboorte OK\n");
 342
 343         /* creating publishers */
 344         robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 345         robottype_publisher_motion_status_create(&orte, NULL, NULL);
 346         robottype_publisher_odo_data_create(&orte, NULL, NULL);
 347         robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 348         robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 349         robottype_publisher_robot_switches_create(&orte, NULL, NULL);
 350         robottype_publisher_robot_bumpers_create(&orte, NULL, NULL);
 351         robottype_publisher_cl_sensor_status_create(&orte, NULL, NULL);
 352         printf("Publishers OK\n");
 353
 354         /* creating subscribers */
 355         robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_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_cl_sensor_cmd_create(&orte, rcv_cl_sensor_cmd_cb, &orte);
 359         printf("subscribers OK\n");
 360
 361         /* main loop */
 362         for(;;) {
 363                 struct pollfd pfd[1] = {{sock, POLLIN, 0}};
 364
 365                 ret = poll(pfd, 1, -1);
 366                 if (ret < 0) {
 367                         perror("cand: poll() error");
 368                         goto err;
 369                 }
 370
 371                 /* timeout expired, nothing to be done */
 372                 if (ret == 0)
 373                         continue;
 374
 375                 /* read data from SocketCAN */
 376                 size = read(sock, &frame, sizeof(struct can_frame));
 377
 378                 /* parse data */
 379                 if (size > 0)
 380                         cand_parse_frame(&orte, frame);
 381         }
 382         return EXIT_SUCCESS;
 383
 384 err:
 385         close(sock);
 386         return 1;
 387 }