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