src/displayd/displayd.c

   1 /**
   2  * @file   displayd.c
   3  * @date   10/04/19
   4  *
   5  * @brief  Display-ORTE bridge
   6  *
   7  * Copyright: (c) 2010 CTU Dragons
   8  *            CTU FEE - Department of Control Engineering
   9  * License: GNU GPL v.2
  10  */
  11
  12 #include <string.h>
  13
  14 #include <orte.h>
  15 #include <roboorte_robottype.h>
  16 //#include <pthread.h>
  17 #include <time.h>
  18 #include "uoled.h"
  19 #include "displayd.h"
  20 #include <poll.h>
  21 #include <unistd.h>
  22 #include <stdlib.h>
  23 #include <signal.h>
  24 #include <stdbool.h>
  25 #include <can_msg_def.h>
  26
  27 struct sercom_data* sercom;
  28 int interrupt = 0;
  29
  30 static void sig_handler(int sig)
  31 {
  32         interrupt = 1;
  33 }
  34
  35
  36 /**
  37  * Subtract the `struct timespec' values X and Y,
  38  *  storing the result in RESULT (result = x - y).
  39  *  Return 1 if the difference is negative, otherwise 0.
  40  */
  41 int timespec_subtract (struct timespec *result,
  42                    struct timespec *x,
  43                    struct timespec *y)
  44 {
  45   /* Perform the carry for the later subtraction by updating Y. */
  46   if (x->tv_nsec < y->tv_nsec) {
  47     int num_sec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
  48     y->tv_nsec -= 1000000000 * num_sec;
  49     y->tv_sec += num_sec;
  50   }
  51   if (x->tv_nsec - y->tv_nsec > 1000000000) {
  52     int num_sec = (x->tv_nsec - y->tv_nsec) / 1000000000;
  53     y->tv_nsec += 1000000000 * num_sec;
  54     y->tv_sec -= num_sec;
  55   }
  56
  57   /* Compute the time remaining to wait.
  58      `tv_nsec' is certainly positive. */
  59   result->tv_sec = x->tv_sec - y->tv_sec;
  60   result->tv_nsec = x->tv_nsec - y->tv_nsec;
  61
  62   /* Return 1 if result is negative. */
  63   return x->tv_sec < y->tv_sec;
  64 }
  65
  66 bool miliseconds_elapsed_since(struct timespec *t, unsigned miliseconds)
  67 {
  68         struct timespec now, diff;
  69         unsigned long long elapsed;
  70
  71         if (t->tv_sec == 0 && t->tv_nsec == 0)
  72                 return true;    /* Always elapsed after program start */
  73
  74         clock_gettime(CLOCK_MONOTONIC, &now);
  75         timespec_subtract(&diff, &now, t);
  76         elapsed = diff.tv_sec * 1000 + diff.tv_nsec/1000000;
  77         return elapsed > miliseconds;
  78 }
  79
  80
  81
  82 void rcv_pwr_voltage_cb (const ORTERecvInfo *info, void *vinstance,
  83                         void *recvCallBackParam)
  84 {
  85         UDE_hw_status_t status = STATUS_FAILED;
  86         static UDE_hw_status_t last_status;
  87         static struct timespec last_sent;
  88         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
  89         switch (info->status) {
  90                 case NEW_DATA:
  91                         uoled_display_voltage(orte_data->pwr_voltage.voltage33,orte_data->pwr_voltage.voltage50,
  92                                                 orte_data->pwr_voltage.voltage80, orte_data->pwr_voltage.voltageBAT);
  93                         status = STATUS_OK;
  94                         break;
  95                 case DEADLINE:
  96                         uoled_display_voltage(0,0,0,0);
  97                         status = STATUS_FAILED;
  98                         break;
  99         }
 100         if (status != last_status ||
 101             miliseconds_elapsed_since(&last_sent, 1000)) {
 102                 uoled_display_status(PWR, status);
 103                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 104         }
 105         last_status = status;
 106 }
 107
 108 void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
 109                         void *recvCallBackParam)
 110 {
 111         UDE_hw_status_t status = STATUS_FAILED;
 112         static UDE_hw_status_t last_status;
 113         static struct timespec last_sent;
 114         //struct odo_data_type *instance = (struct odo_data_type *)vinstance;
 115         switch (info->status) {
 116                 case NEW_DATA:
 117                         status = STATUS_OK;
 118                         break;
 119                 case DEADLINE:
 120                         status = STATUS_FAILED;
 121                         break;
 122         }
 123         /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
 124         if (status != last_status ||
 125             miliseconds_elapsed_since(&last_sent, 1000)) {
 126                 uoled_display_status(ODO, status);
 127                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 128         }
 129         last_status = status;
 130 }
 131
 132 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 133                         void *recvCallBackParam)
 134 {
 135         UDE_hw_status_t status = STATUS_FAILED;
 136         static UDE_hw_status_t last_status;
 137         static struct timespec last_sent;
 138         struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
 139
 140         switch (info->status) {
 141                 case NEW_DATA:
 142                         switch (instance->start_condition) {
 143                                 case POWER_ON:
 144                                 case COMPETITION_STARTED:
 145                                         status = STATUS_WARNING;
 146                                         break;
 147                                 case START_PLUGGED_IN:
 148                                         status = STATUS_OK;
 149                                         break;
 150                         }
 151                         break;
 152                 case DEADLINE:
 153                         status = STATUS_FAILED;
 154                         break;
 155         }
 156         if (status != last_status ||
 157             miliseconds_elapsed_since(&last_sent, 1000)) {
 158                 uoled_display_status(STA, status);
 159                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 160         }
 161         last_status = status;
 162 }
 163
 164 void rcv_robot_swicthes_cb(const ORTERecvInfo *info, void *vinstance,
 165                            void *recvCallBackParam)
 166 {
 167         UDE_hw_status_t status = STATUS_FAILED;
 168         static char last_color;
 169         static struct timespec last_sent;
 170         struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
 171
 172         switch (info->status) {
 173                 case NEW_DATA:
 174 //                      if (instance->team_color != last_color ||
 175 //                          miliseconds_elapsed_since(&last_sent, 1000)) {
 176 //                              uoled_display_color(instance->team_color);
 177 //                              clock_gettime(CLOCK_MONOTONIC, &last_sent);
 178 //                      }
 179 //                      last_color = instance->team_color;
 180                 case DEADLINE:
 181                         status = STATUS_FAILED;
 182                         break;
 183         }
 184 }
 185
 186 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
 187                         void *recvCallBackParam)
 188 {
 189         struct motion_status_type *m = (struct motion_status_type *)vinstance;
 190         UDE_hw_status_t status = STATUS_FAILED;
 191         static UDE_hw_status_t last_status;
 192         static struct timespec last_sent;
 193         switch (info->status) {
 194                 case NEW_DATA:
 195                         if (m->err_left == 0 && m->err_right == 0)
 196                                 status = STATUS_OK;
 197                         else
 198                                 status = STATUS_WARNING;
 199                         break;
 200                 case DEADLINE:
 201                         status = STATUS_FAILED;
 202                         break;
 203         }
 204         /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
 205         if (status != last_status ||
 206             miliseconds_elapsed_since(&last_sent, 1000)) {
 207                 uoled_display_status(MOT, status);
 208                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 209         }
 210         last_status = status;
 211 }
 212
 213 void rcv_crane_status_cb(const ORTERecvInfo *info, void *vinstance,
 214                         void *recvCallBackParam)
 215 {
 216         struct crane_status_type *m = (struct crane_status_type *)vinstance;
 217         UDE_hw_status_t status = STATUS_FAILED;
 218         static UDE_hw_status_t last_status;
 219         static struct timespec last_sent;
 220         switch (info->status) {
 221                 case NEW_DATA:
 222                         status = STATUS_OK;
 223                         break;
 224                 case DEADLINE:
 225                         status = STATUS_FAILED;
 226                         break;
 227         }
 228         /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
 229         if (status != last_status ||
 230             miliseconds_elapsed_since(&last_sent, 1000)) {
 231                 uoled_display_status(MAN, status);
 232                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 233         }
 234         last_status = status;
 235 }
 236
 237 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 238                         void *recvCallBackParam)
 239 {
 240         UDE_hw_status_t status = STATUS_FAILED;
 241         static UDE_hw_status_t last_status;
 242         static struct timespec last_sent;
 243         struct camera_result_type *instance = (struct camera_result_type *)vinstance;
 244         char valid;
 245         switch (info->status) {
 246                 case NEW_DATA:
 247                         if (instance->error == 0)
 248                                 status = STATUS_OK;
 249                         else
 250                                 status = STATUS_WARNING;
 251                         break;
 252                 case DEADLINE:
 253                         status = STATUS_FAILED;
 254                         break;
 255         }
 256         if (status != last_status ||
 257             miliseconds_elapsed_since(&last_sent, 1000)) {
 258                 uoled_display_status(CAM, status);
 259                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 260         }
 261         last_status = status;
 262 }
 263
 264 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 265                         void *recvCallBackParam)
 266 {
 267         UDE_hw_status_t status = STATUS_FAILED;
 268         static UDE_hw_status_t last_status;
 269         static struct timespec last_sent;
 270         //struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 271         switch (info->status) {
 272                 case NEW_DATA:
 273                         status = STATUS_OK;
 274                         break;
 275                 case DEADLINE:
 276                         status = STATUS_FAILED;
 277                         break;
 278         }
 279         if (status != last_status ||
 280             miliseconds_elapsed_since(&last_sent, 1000)) {
 281                 uoled_display_status(HOK, status);
 282                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 283         }
 284         last_status = status;
 285 }
 286
 287 void rcv_est_pos_best_cb (const ORTERecvInfo *info, void *vinstance,
 288                         void *recvCallBackParam)
 289 {
 290         UDE_hw_status_t status = STATUS_FAILED;
 291         static UDE_hw_status_t last_status;
 292         static struct timespec last_sent;
 293         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 294         switch (info->status) {
 295                 case NEW_DATA:
 296                         status = STATUS_OK;
 297                         break;
 298                 case DEADLINE:
 299                         status = STATUS_FAILED;
 300                         break;
 301         }
 302         if (status != last_status ||
 303             miliseconds_elapsed_since(&last_sent, 100)) {
 304                 uoled_display_status(APP, status);
 305                 if (status == STATUS_OK)
 306                         uoled_display_position(orte_data->est_pos_best.x, orte_data->est_pos_best.y, orte_data->est_pos_best.phi);
 307                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 308         }
 309         last_status = status;
 310 }
 311
 312 void rcv_fsm_main_cb(const ORTERecvInfo *info, void *vinstance,
 313                        void *recvCallBackParam)
 314 {
 315         char status[MGS_LENGTH_DISPLAY_SM];
 316         static char last_status[MGS_LENGTH_DISPLAY_SM];
 317         static struct timespec last_sent;
 318         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 319         switch (info->status) {
 320                 case NEW_DATA:
 321                         strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
 322                         status[MGS_LENGTH_DISPLAY_SM-1]='\0';
 323                         //uoled_display_fsm(FSM_MAIN, fsm_state->state_name);
 324                         break;
 325                 case DEADLINE:
 326                         strcpy(status, "?");
 327                         //uoled_display_fsm(FSM_MAIN, "?");
 328                         break;
 329         }
 330         if (strcmp(status,last_status) != 0 ||
 331             miliseconds_elapsed_since(&last_sent, 1000)) {
 332                 uoled_display_fsm(FSM_MAIN, status);
 333                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 334         }
 335         strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
 336 }
 337
 338 void rcv_fsm_act_cb(const ORTERecvInfo *info, void *vinstance,
 339                      void *recvCallBackParam)
 340 {
 341         char status[MGS_LENGTH_DISPLAY_SM];
 342         static char last_status[MGS_LENGTH_DISPLAY_SM];
 343         static struct timespec last_sent;
 344         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 345         switch (info->status) {
 346                 case NEW_DATA:
 347                         strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
 348                         status[MGS_LENGTH_DISPLAY_SM-1]='\0';
 349                         break;
 350                 case DEADLINE:
 351                         strcpy(status, "?");
 352                         break;
 353         }
 354         if (strcmp(status,last_status) != 0 ||
 355             miliseconds_elapsed_since(&last_sent, 1000)) {
 356                 uoled_display_fsm(FSM_ACT, status);
 357                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 358         }
 359         strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
 360 }
 361
 362 void rcv_fsm_motion_cb(const ORTERecvInfo *info, void *vinstance,
 363                      void *recvCallBackParam)
 364 {
 365         char status[MGS_LENGTH_DISPLAY_SM];
 366         static char last_status[MGS_LENGTH_DISPLAY_SM];
 367         static struct timespec last_sent;
 368         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 369         switch (info->status) {
 370                 case NEW_DATA:
 371                         strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
 372                         status[MGS_LENGTH_DISPLAY_SM-1]='\0';
 373                         break;
 374                 case DEADLINE:
 375                         strcpy(status, "?");
 376                         break;
 377         }
 378         if (strcmp(status,last_status) != 0 ||
 379             miliseconds_elapsed_since(&last_sent, 1000)) {
 380                 uoled_display_fsm(FSM_MOVE, status);
 381                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 382         }
 383         strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
 384 }
 385
 386
 387
 388
 389 int main(int argc, char *argv[])
 390 {
 391         //fd_set read_fd_set;
 392         //struct timeval timeout;
 393         int ret;
 394         //int size;
 395
 396         struct robottype_orte_data orte;
 397         char * tty;
 398
 399         signal(SIGINT, sig_handler);
 400         signal(SIGTERM, sig_handler);
 401
 402         //struct can_frame frame;
 403         tty = getenv("DISPLAY_TTY");
 404         if (!tty){
 405                 tty="/dev/ttyUSB0";
 406                 printf("displayd: enviroment variable 'DISPLAY_TTY' was not set, using default value '%s'.\n", tty);
 407         }
 408
 409         sercom = uoled_sercom_init(tty, NULL);
 410         if (strcmp(tty, "/dev/null") == 0 || sercom != NULL) {
 411         // :wq
 412                 printf("displayd: init serial communication OK\n");
 413         } else {
 414                 fprintf(stderr, "displayd: init serial communication failed\n");
 415 //#ifdef COMPETITION
 416                 /* Sleep in order to that not be respawn too fast by init */
 417                 sleep(3);
 418                 return 1;
 419 //#endif
 420         }
 421
 422         orte.strength = 1;
 423
 424         /* orte initialization */
 425         if(robottype_roboorte_init(&orte)) {
 426                 fprintf(stderr, "displayd: Roboorte initialization failed! Exiting...\n");
 427 #ifdef COMPETITION
 428                 /* Sleep in order to that not be respawn too fast by init */
 429                 sleep(3);
 430 #endif
 431                 return 1;
 432         }
 433         else
 434                 printf("displayd: Roboorte OK\n");
 435
 436         /* creating publishers */
 437         //robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 438         //robottype_publisher_motion_status_create(&orte, NULL, NULL);
 439         //robottype_publisher_odo_data_create(&orte, NULL, NULL);
 440         //robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 441         //robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 442         //robottype_publisher_corr_trig_create(&orte, NULL, NULL);
 443         //robottype_publisher_corr_distances_create(&orte, NULL, NULL);
 444         //robottype_publisher_actuator_status_create(&orte, NULL, NULL);
 445         //printf("Publishers OK\n");
 446
 447         /* creating subscribers */
 448         robottype_subscriber_odo_data_create(&orte, rcv_odo_data_cb, &orte);
 449         robottype_subscriber_motion_status_create(&orte, rcv_motion_status_cb, NULL);
 450         robottype_subscriber_crane_status_create(&orte, rcv_crane_status_cb, NULL);
 451         robottype_subscriber_pwr_voltage_create(&orte, rcv_pwr_voltage_cb, &orte);
 452         robottype_subscriber_est_pos_best_create(&orte, rcv_est_pos_best_cb, &orte);
 453         //robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
 454         //robottype_subscriber_motion_speed_create(&orte,       rcv_motion_speed_cb, &orte);
 455         //robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
 456         robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
 457         robottype_subscriber_camera_result_create(&orte, rcv_camera_result_cb, &orte);
 458         robottype_subscriber_fsm_main_create(&orte, rcv_fsm_main_cb, &orte);
 459         robottype_subscriber_fsm_act_create(&orte, rcv_fsm_act_cb, &orte);
 460         robottype_subscriber_fsm_motion_create(&orte, rcv_fsm_motion_cb, &orte);
 461         robottype_subscriber_robot_cmd_create(&orte, rcv_robot_cmd_cb, &orte);
 462         robottype_subscriber_robot_switches_create(&orte, rcv_robot_swicthes_cb, &orte);
 463
 464         printf("displayd: subscribers created OK\n");
 465
 466
 467         UDE_recieve_cmd_t rcmd;
 468         struct pollfd pfd[1] = {{
 469                         .fd = sercom->fd,
 470                         .events = POLLIN,
 471                 }};
 472
 473         /* main loop */
 474         while(!interrupt)
 475         {
 476                 if (strcmp(tty, "/dev/null") != 0) {
 477                         ret = poll(pfd, 1, 500/*ms*/);
 478                         if (ret > 0) {
 479                                 ret = uoled_recieve_cmd(&rcmd);
 480 /*                              orte.display_cmd.command=rcmd; */
 481 /*                              if (! rcmd){ */
 482 /*                                      ORTEPublicationSend(orte.publication_display_cmd); */
 483 /*                                      printf("displayd: publishing command recieved from display [ 0x%2x ]\n", rcmd); */
 484
 485 /*                              } */
 486                         }
 487                         if (ret < 0) {
 488                                 perror("Error: read from display");
 489                                 break;
 490                         }
 491                 } else {
 492                         usleep(500/*ms*/*1000);
 493                         ret = 0;
 494                 }
 495                 if (ret == 0)
 496                         uoled_display_alive();
 497         }
 498
 499         // deinitialization
 500         printf("displayd: closing serial communication...\n");
 501         uoled_sercom_close();
 502 //      printf("displayd: destroying display_cmd publisher...\n");
 503 //      robottype_publisher_display_cmd_destroy(&orte);
 504         printf("displayd: destroying orte instance...\n");
 505         robottype_roboorte_destroy(&orte);
 506
 507         if (!interrupt) {
 508                 printf("Waiting 3 seconds\n");
 509                 sleep(3);
 510         }
 511
 512         printf("displayd: finished!\n");
 513         return 0;
 514 }