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_motion_irc_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         switch (info->status) {
 137                 case NEW_DATA:
 138                         status = STATUS_OK;
 139                         break;
 140                 case DEADLINE:
 141                         status = STATUS_FAILED;
 142                         break;
 143         }
 144         /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
 145         if (status != last_status ||
 146             miliseconds_elapsed_since(&last_sent, 1000)) {
 147                 uoled_display_status(MOT, status);
 148                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 149         }
 150         last_status = status;
 151 }
 152
 153 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
 154                         void *recvCallBackParam)
 155 {
 156         struct motion_status_type *m = (struct motion_status_type *)vinstance;
 157         UDE_hw_status_t status = STATUS_FAILED;
 158         static UDE_hw_status_t last_status;
 159         static struct timespec last_sent;
 160         switch (info->status) {
 161                 case NEW_DATA:
 162                         if (m->err_left == 0 && m->err_right == 0)
 163                                 status = STATUS_OK;
 164                         else
 165                                 status = STATUS_WARNING;
 166                         break;
 167                 case DEADLINE:
 168                         status = STATUS_FAILED;
 169                         break;
 170         }
 171         /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
 172         if (status != last_status ||
 173             miliseconds_elapsed_since(&last_sent, 1000)) {
 174                 uoled_display_status(MOT, status);
 175                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 176         }
 177         last_status = status;
 178 }
 179
 180 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 181                         void *recvCallBackParam)
 182 {
 183         UDE_hw_status_t status = STATUS_FAILED;
 184         static UDE_hw_status_t last_status;
 185         static struct timespec last_sent;
 186         //struct camera_result_type *instance = (struct camera_result_type *)vinstance;
 187         switch (info->status) {
 188                 case NEW_DATA:
 189                         status = STATUS_OK;
 190                         break;
 191                 case DEADLINE:
 192                         status = STATUS_FAILED;
 193                         break;
 194         }
 195         if (status != last_status ||
 196             miliseconds_elapsed_since(&last_sent, 1000)) {
 197                 uoled_display_status(CAM, status);
 198                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 199         }
 200         last_status = status;
 201 }
 202
 203 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 204                         void *recvCallBackParam)
 205 {
 206         UDE_hw_status_t status = STATUS_FAILED;
 207         static UDE_hw_status_t last_status;
 208         static struct timespec last_sent;
 209         //struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 210         switch (info->status) {
 211                 case NEW_DATA:
 212                         status = STATUS_OK;
 213                         break;
 214                 case DEADLINE:
 215                         status = STATUS_FAILED;
 216                         break;
 217         }
 218         if (status != last_status ||
 219             miliseconds_elapsed_since(&last_sent, 1000)) {
 220                 uoled_display_status(HOK, status);
 221                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 222         }
 223         last_status = status;
 224 }
 225
 226 void rcv_est_pos_best_cb (const ORTERecvInfo *info, void *vinstance,
 227                         void *recvCallBackParam)
 228 {
 229         UDE_hw_status_t status = STATUS_FAILED;
 230         static UDE_hw_status_t last_status;
 231         static struct timespec last_sent;
 232         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 233         switch (info->status) {
 234                 case NEW_DATA:
 235                         status = STATUS_OK;
 236                         break;
 237                 case DEADLINE:
 238                         status = STATUS_FAILED;
 239                         break;
 240         }
 241         if (status != last_status ||
 242             miliseconds_elapsed_since(&last_sent, 100)) {
 243                 uoled_display_status(APP, status);
 244                 if (status == STATUS_OK)
 245                         uoled_display_position(orte_data->est_pos_best.x, orte_data->est_pos_best.y, orte_data->est_pos_best.phi);
 246                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 247         }
 248         last_status = status;
 249 }
 250
 251 void rcv_fsm_main_cb(const ORTERecvInfo *info, void *vinstance,
 252                        void *recvCallBackParam)
 253 {
 254         char status[MGS_LENGTH_DISPLAY_SM];
 255         static char last_status[MGS_LENGTH_DISPLAY_SM];
 256         static struct timespec last_sent;
 257         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 258         switch (info->status) {
 259                 case NEW_DATA:
 260                         strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
 261                         status[MGS_LENGTH_DISPLAY_SM-1]='\0';
 262                         //uoled_display_fsm(FSM_MAIN, fsm_state->state_name);
 263                         break;
 264                 case DEADLINE:
 265                         strcpy(status, "?");
 266                         //uoled_display_fsm(FSM_MAIN, "?");
 267                         break;
 268         }
 269         if (strcmp(status,last_status) != 0 ||
 270             miliseconds_elapsed_since(&last_sent, 1000)) {
 271                 uoled_display_fsm(FSM_MAIN, status);
 272                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 273         }
 274         strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
 275 }
 276
 277 void rcv_fsm_act_cb(const ORTERecvInfo *info, void *vinstance,
 278                      void *recvCallBackParam)
 279 {
 280         char status[MGS_LENGTH_DISPLAY_SM];
 281         static char last_status[MGS_LENGTH_DISPLAY_SM];
 282         static struct timespec last_sent;
 283         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 284         switch (info->status) {
 285                 case NEW_DATA:
 286                         strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
 287                         status[MGS_LENGTH_DISPLAY_SM-1]='\0';
 288                         break;
 289                 case DEADLINE:
 290                         strcpy(status, "?");
 291                         break;
 292         }
 293         if (strcmp(status,last_status) != 0 ||
 294             miliseconds_elapsed_since(&last_sent, 1000)) {
 295                 uoled_display_fsm(FSM_ACT, status);
 296                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 297         }
 298         strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
 299 }
 300
 301 void rcv_fsm_motion_cb(const ORTERecvInfo *info, void *vinstance,
 302                      void *recvCallBackParam)
 303 {
 304         char status[MGS_LENGTH_DISPLAY_SM];
 305         static char last_status[MGS_LENGTH_DISPLAY_SM];
 306         static struct timespec last_sent;
 307         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 308         switch (info->status) {
 309                 case NEW_DATA:
 310                         strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
 311                         status[MGS_LENGTH_DISPLAY_SM-1]='\0';
 312                         break;
 313                 case DEADLINE:
 314                         strcpy(status, "?");
 315                         break;
 316         }
 317         if (strcmp(status,last_status) != 0 ||
 318             miliseconds_elapsed_since(&last_sent, 1000)) {
 319                 uoled_display_fsm(FSM_MOVE, status);
 320                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 321         }
 322         strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
 323 }
 324
 325
 326
 327
 328 int main(int argc, char *argv[])
 329 {
 330         //fd_set read_fd_set;
 331         //struct timeval timeout;
 332         int ret;
 333         //int size;
 334
 335         struct robottype_orte_data orte;
 336         char * tty;
 337
 338         signal(SIGINT, sig_handler);
 339         signal(SIGTERM, sig_handler);
 340
 341         //struct can_frame frame;
 342         tty = getenv("DISPLAY_TTY");
 343         if (!tty){
 344                 tty="/dev/ttyUSB0";
 345                 printf("displayd: enviroment variable 'DISPLAY_TTY' was not set, using default value '%s'.\n", tty);
 346         }
 347
 348         sercom = uoled_sercom_init(tty, NULL);
 349         if (strcmp(tty, "/dev/null") != 0 && sercom) {
 350         // :wq
 351                 printf("displayd: init serial communication OK\n");
 352         } else {
 353                 fprintf(stderr, "displayd: init serial communication failed\n");
 354 #ifdef COMPETITION
 355                 /* Sleep in order to that not be respawn too fast by init */
 356                 sleep(3);
 357                 return 1;
 358 #endif
 359         }
 360
 361         orte.strength = 1;
 362
 363         /* orte initialization */
 364         if(robottype_roboorte_init(&orte)) {
 365                 fprintf(stderr, "displayd: Roboorte initialization failed! Exiting...\n");
 366 #ifdef COMPETITION
 367                 /* Sleep in order to that not be respawn too fast by init */
 368                 sleep(1);
 369 #endif
 370                 return 1;
 371         }
 372         else
 373                 printf("displayd: Roboorte OK\n");
 374
 375         /* creating publishers */
 376         //robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 377         //robottype_publisher_motion_status_create(&orte, NULL, NULL);
 378         //robottype_publisher_odo_data_create(&orte, NULL, NULL);
 379         //robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 380         //robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 381         //robottype_publisher_corr_trig_create(&orte, NULL, NULL);
 382         //robottype_publisher_corr_distances_create(&orte, NULL, NULL);
 383         //robottype_publisher_actuator_status_create(&orte, NULL, NULL);
 384         //printf("Publishers OK\n");
 385
 386         /* creating subscribers */
 387         robottype_subscriber_odo_data_create(&orte, rcv_odo_data_cb, &orte);
 388         robottype_subscriber_motion_status_create(&orte, rcv_motion_status_cb, NULL);
 389         //robottype_subscriber_motion_irc_create(&orte, rcv_motion_irc_cb, &orte);
 390         robottype_subscriber_pwr_voltage_create(&orte, rcv_pwr_voltage_cb, &orte);
 391         robottype_subscriber_est_pos_best_create(&orte, rcv_est_pos_best_cb, &orte);
 392         //robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
 393         //robottype_subscriber_motion_speed_create(&orte,       rcv_motion_speed_cb, &orte);
 394         //robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
 395         robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
 396         robottype_subscriber_camera_result_create(&orte, rcv_camera_result_cb, &orte);
 397         robottype_subscriber_fsm_main_create(&orte, rcv_fsm_main_cb, &orte);
 398         robottype_subscriber_fsm_act_create(&orte, rcv_fsm_act_cb, &orte);
 399         robottype_subscriber_fsm_motion_create(&orte, rcv_fsm_motion_cb, &orte);
 400
 401         printf("displayd: subscribers created OK\n");
 402
 403
 404         UDE_recieve_cmd_t rcmd;
 405         struct pollfd pfd[1] = {{
 406                         .fd = sercom->fd,
 407                         .events = POLLIN,
 408                 }};
 409
 410         /* main loop */
 411         while(!interrupt)
 412         {
 413                 if (strcmp(tty, "/dev/null") != 0) {
 414                         ret = poll(pfd, 1, 500/*ms*/);
 415                         if (ret > 0) {
 416                                 rcmd = uoled_recieve_cmd();
 417                                 orte.display_cmd.command=rcmd;
 418                                 if (! rcmd){
 419                                         ORTEPublicationSend(orte.publication_display_cmd);
 420                                         printf("displayd: publishing command recieved from display [ 0x%2x ]\n", rcmd);
 421
 422                                 }
 423                         }
 424                 } else {
 425                         usleep(500/*ms*/*1000);
 426                         ret = 0;
 427                 }
 428                 if (ret == 0)
 429                         uoled_display_alive();
 430         }
 431
 432         // deinitialization
 433         printf("displayd: closing serial communication...\n");
 434         uoled_sercom_close();
 435 //      printf("displayd: destroying display_cmd publisher...\n");
 436 //      robottype_publisher_display_cmd_destroy(&orte);
 437         printf("displayd: destroying orte instance...\n");
 438         robottype_roboorte_destroy(&orte);
 439
 440         printf("displayd: finished!\n");
 441         return 0;
 442 }