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