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         switch (info->status) {
 241                 case NEW_DATA:
 242                         status = STATUS_OK;
 243                         break;
 244                 case DEADLINE:
 245                         status = STATUS_FAILED;
 246                         break;
 247         }
 248         if (status != last_status ||
 249             miliseconds_elapsed_since(&last_sent, 1000)) {
 250                 uoled_display_status(CAM, status);
 251                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 252         }
 253         last_status = status;
 254 }
 255
 256 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 257                         void *recvCallBackParam)
 258 {
 259         UDE_hw_status_t status = STATUS_FAILED;
 260         static UDE_hw_status_t last_status;
 261         static struct timespec last_sent;
 262         //struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 263         switch (info->status) {
 264                 case NEW_DATA:
 265                         status = STATUS_OK;
 266                         break;
 267                 case DEADLINE:
 268                         status = STATUS_FAILED;
 269                         break;
 270         }
 271         if (status != last_status ||
 272             miliseconds_elapsed_since(&last_sent, 1000)) {
 273                 uoled_display_status(HOK, status);
 274                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 275         }
 276         last_status = status;
 277 }
 278
 279 void rcv_est_pos_best_cb (const ORTERecvInfo *info, void *vinstance,
 280                         void *recvCallBackParam)
 281 {
 282         UDE_hw_status_t status = STATUS_FAILED;
 283         static UDE_hw_status_t last_status;
 284         static struct timespec last_sent;
 285         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 286         switch (info->status) {
 287                 case NEW_DATA:
 288                         status = STATUS_OK;
 289                         break;
 290                 case DEADLINE:
 291                         status = STATUS_FAILED;
 292                         break;
 293         }
 294         if (status != last_status ||
 295             miliseconds_elapsed_since(&last_sent, 100)) {
 296                 uoled_display_status(APP, status);
 297                 if (status == STATUS_OK)
 298                         uoled_display_position(orte_data->est_pos_best.x, orte_data->est_pos_best.y, orte_data->est_pos_best.phi);
 299                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 300         }
 301         last_status = status;
 302 }
 303
 304 void rcv_fsm_main_cb(const ORTERecvInfo *info, void *vinstance,
 305                        void *recvCallBackParam)
 306 {
 307         char status[MGS_LENGTH_DISPLAY_SM];
 308         static char last_status[MGS_LENGTH_DISPLAY_SM];
 309         static struct timespec last_sent;
 310         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 311         switch (info->status) {
 312                 case NEW_DATA:
 313                         strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
 314                         status[MGS_LENGTH_DISPLAY_SM-1]='\0';
 315                         //uoled_display_fsm(FSM_MAIN, fsm_state->state_name);
 316                         break;
 317                 case DEADLINE:
 318                         strcpy(status, "?");
 319                         //uoled_display_fsm(FSM_MAIN, "?");
 320                         break;
 321         }
 322         if (strcmp(status,last_status) != 0 ||
 323             miliseconds_elapsed_since(&last_sent, 1000)) {
 324                 uoled_display_fsm(FSM_MAIN, status);
 325                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 326         }
 327         strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
 328 }
 329
 330 void rcv_fsm_act_cb(const ORTERecvInfo *info, void *vinstance,
 331                      void *recvCallBackParam)
 332 {
 333         char status[MGS_LENGTH_DISPLAY_SM];
 334         static char last_status[MGS_LENGTH_DISPLAY_SM];
 335         static struct timespec last_sent;
 336         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 337         switch (info->status) {
 338                 case NEW_DATA:
 339                         strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
 340                         status[MGS_LENGTH_DISPLAY_SM-1]='\0';
 341                         break;
 342                 case DEADLINE:
 343                         strcpy(status, "?");
 344                         break;
 345         }
 346         if (strcmp(status,last_status) != 0 ||
 347             miliseconds_elapsed_since(&last_sent, 1000)) {
 348                 uoled_display_fsm(FSM_ACT, status);
 349                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 350         }
 351         strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
 352 }
 353
 354 void rcv_fsm_motion_cb(const ORTERecvInfo *info, void *vinstance,
 355                      void *recvCallBackParam)
 356 {
 357         char status[MGS_LENGTH_DISPLAY_SM];
 358         static char last_status[MGS_LENGTH_DISPLAY_SM];
 359         static struct timespec last_sent;
 360         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 361         switch (info->status) {
 362                 case NEW_DATA:
 363                         strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
 364                         status[MGS_LENGTH_DISPLAY_SM-1]='\0';
 365                         break;
 366                 case DEADLINE:
 367                         strcpy(status, "?");
 368                         break;
 369         }
 370         if (strcmp(status,last_status) != 0 ||
 371             miliseconds_elapsed_since(&last_sent, 1000)) {
 372                 uoled_display_fsm(FSM_MOVE, status);
 373                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 374         }
 375         strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
 376 }
 377
 378
 379
 380
 381 int main(int argc, char *argv[])
 382 {
 383         //fd_set read_fd_set;
 384         //struct timeval timeout;
 385         int ret;
 386         //int size;
 387
 388         struct robottype_orte_data orte;
 389         char * tty;
 390
 391         signal(SIGINT, sig_handler);
 392         signal(SIGTERM, sig_handler);
 393
 394         //struct can_frame frame;
 395         tty = getenv("DISPLAY_TTY");
 396         if (!tty){
 397                 tty="/dev/ttyUSB0";
 398                 printf("displayd: enviroment variable 'DISPLAY_TTY' was not set, using default value '%s'.\n", tty);
 399         }
 400
 401         sercom = uoled_sercom_init(tty, NULL);
 402         if (strcmp(tty, "/dev/null") != 0 && sercom != NULL) {
 403         // :wq
 404                 printf("displayd: init serial communication OK\n");
 405         } else {
 406                 fprintf(stderr, "displayd: init serial communication failed\n");
 407 //#ifdef COMPETITION
 408                 /* Sleep in order to that not be respawn too fast by init */
 409                 sleep(3);
 410                 return 1;
 411 //#endif
 412         }
 413
 414         orte.strength = 1;
 415
 416         /* orte initialization */
 417         if(robottype_roboorte_init(&orte)) {
 418                 fprintf(stderr, "displayd: Roboorte initialization failed! Exiting...\n");
 419 #ifdef COMPETITION
 420                 /* Sleep in order to that not be respawn too fast by init */
 421                 sleep(3);
 422 #endif
 423                 return 1;
 424         }
 425         else
 426                 printf("displayd: Roboorte OK\n");
 427
 428         /* creating publishers */
 429         //robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 430         //robottype_publisher_motion_status_create(&orte, NULL, NULL);
 431         //robottype_publisher_odo_data_create(&orte, NULL, NULL);
 432         //robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 433         //robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 434         //robottype_publisher_corr_trig_create(&orte, NULL, NULL);
 435         //robottype_publisher_corr_distances_create(&orte, NULL, NULL);
 436         //robottype_publisher_actuator_status_create(&orte, NULL, NULL);
 437         //printf("Publishers OK\n");
 438
 439         /* creating subscribers */
 440         robottype_subscriber_odo_data_create(&orte, rcv_odo_data_cb, &orte);
 441         robottype_subscriber_motion_status_create(&orte, rcv_motion_status_cb, NULL);
 442         robottype_subscriber_vidle_status_create(&orte, rcv_vidle_status_cb, NULL);
 443         robottype_subscriber_pwr_voltage_create(&orte, rcv_pwr_voltage_cb, &orte);
 444         robottype_subscriber_est_pos_best_create(&orte, rcv_est_pos_best_cb, &orte);
 445         //robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
 446         //robottype_subscriber_motion_speed_create(&orte,       rcv_motion_speed_cb, &orte);
 447         //robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
 448         robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
 449         robottype_subscriber_camera_result_create(&orte, rcv_camera_result_cb, &orte);
 450         robottype_subscriber_fsm_main_create(&orte, rcv_fsm_main_cb, &orte);
 451         robottype_subscriber_fsm_act_create(&orte, rcv_fsm_act_cb, &orte);
 452         robottype_subscriber_fsm_motion_create(&orte, rcv_fsm_motion_cb, &orte);
 453         robottype_subscriber_robot_cmd_create(&orte, rcv_robot_cmd_cb, &orte);
 454         robottype_subscriber_robot_switches_create(&orte, rcv_robot_swicthes_cb, &orte);
 455
 456         printf("displayd: subscribers created OK\n");
 457
 458
 459         UDE_recieve_cmd_t rcmd;
 460         struct pollfd pfd[1] = {{
 461                         .fd = sercom->fd,
 462                         .events = POLLIN,
 463                 }};
 464
 465         /* main loop */
 466         while(!interrupt)
 467         {
 468                 if (strcmp(tty, "/dev/null") != 0) {
 469                         ret = poll(pfd, 1, 500/*ms*/);
 470                         if (ret > 0) {
 471                                 ret = uoled_recieve_cmd(&rcmd);
 472 /*                              orte.display_cmd.command=rcmd; */
 473 /*                              if (! rcmd){ */
 474 /*                                      ORTEPublicationSend(orte.publication_display_cmd); */
 475 /*                                      printf("displayd: publishing command recieved from display [ 0x%2x ]\n", rcmd); */
 476
 477 /*                              } */
 478                         }
 479                         if (ret < 0) {
 480                                 perror("Error: read from display");
 481                                 break;
 482                         }
 483                 } else {
 484                         usleep(500/*ms*/*1000);
 485                         ret = 0;
 486                 }
 487                 if (ret == 0)
 488                         uoled_display_alive();
 489         }
 490
 491         // deinitialization
 492         printf("displayd: closing serial communication...\n");
 493         uoled_sercom_close();
 494 //      printf("displayd: destroying display_cmd publisher...\n");
 495 //      robottype_publisher_display_cmd_destroy(&orte);
 496         printf("displayd: destroying orte instance...\n");
 497         robottype_roboorte_destroy(&orte);
 498
 499         if (!interrupt) {
 500                 printf("Waiting 3 seconds\n");
 501                 sleep(3);
 502         }
 503
 504         printf("displayd: finished!\n");
 505         return 0;
 506 }