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
  24 struct sercom_data* sercom;
  25
  26 /* Subtract the `struct timespec' values X and Y,
  27    storing the result in RESULT (result = x - y).
  28    Return 1 if the difference is negative, otherwise 0.  */
  29
  30 int
  31 timespec_subtract (struct timespec *result,
  32                    struct timespec *x,
  33                    struct timespec *y)
  34 {
  35   /* Perform the carry for the later subtraction by updating Y. */
  36   if (x->tv_nsec < y->tv_nsec) {
  37     int num_sec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
  38     y->tv_nsec -= 1000000000 * num_sec;
  39     y->tv_sec += num_sec;
  40   }
  41   if (x->tv_nsec - y->tv_nsec > 1000000000) {
  42     int num_sec = (x->tv_nsec - y->tv_nsec) / 1000000000;
  43     y->tv_nsec += 1000000000 * num_sec;
  44     y->tv_sec -= num_sec;
  45   }
  46
  47   /* Compute the time remaining to wait.
  48      `tv_nsec' is certainly positive. */
  49   result->tv_sec = x->tv_sec - y->tv_sec;
  50   result->tv_nsec = x->tv_nsec - y->tv_nsec;
  51
  52   /* Return 1 if result is negative. */
  53   return x->tv_sec < y->tv_sec;
  54 }
  55
  56 int miliseconds_since(struct timespec *t)
  57 {
  58         struct timespec now, diff;
  59
  60         clock_gettime(CLOCK_MONOTONIC, &now);
  61         timespec_subtract(&diff, t, &now);
  62         return diff.tv_sec * 1000 + diff.tv_nsec/1000000;
  63 }
  64
  65
  66
  67 void rcv_pwr_voltage_cb (const ORTERecvInfo *info, void *vinstance,
  68                         void *recvCallBackParam)
  69 {
  70         UDE_hw_status_t status = STATUS_FAILED;
  71         static UDE_hw_status_t last_status;
  72         static struct timespec last_sent;
  73         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
  74         switch (info->status) {
  75                 case NEW_DATA:
  76                         uoled_display_voltage(&orte_data->pwr_voltage);
  77                         status = STATUS_OK;
  78                         break;
  79                 case DEADLINE:
  80                         status = STATUS_FAILED;
  81                         break;
  82         }
  83         if (status != last_status ||
  84             miliseconds_since(&last_sent) > 1000) {
  85                 uoled_display_status(PWR, STATUS_OK);
  86                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
  87         }
  88         last_status = status;
  89 }
  90
  91 void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
  92                         void *recvCallBackParam)
  93 {
  94         UDE_hw_status_t status = STATUS_FAILED;
  95         static UDE_hw_status_t last_status;
  96         static struct timespec last_sent;
  97         //struct odo_data_type *instance = (struct odo_data_type *)vinstance;
  98         switch (info->status) {
  99                 case NEW_DATA:
 100                         status = STATUS_OK;
 101                         break;
 102                 case DEADLINE:
 103                         status = STATUS_FAILED;
 104                         break;
 105         }
 106         /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
 107         if (status != last_status ||
 108             miliseconds_since(&last_sent) > 1000) {
 109                 uoled_display_status(ODO, STATUS_OK);
 110                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 111         }
 112         last_status = status;
 113 }
 114
 115 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
 116                         void *recvCallBackParam)
 117 {
 118         UDE_hw_status_t status = STATUS_FAILED;
 119         static UDE_hw_status_t last_status;
 120         static struct timespec last_sent;
 121         switch (info->status) {
 122                 case NEW_DATA:
 123                         status = STATUS_OK;
 124                         break;
 125                 case DEADLINE:
 126                         status = STATUS_FAILED;
 127                         break;
 128         }
 129         /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
 130         if (status != last_status ||
 131             miliseconds_since(&last_sent) > 1000) {
 132                 uoled_display_status(MOT, STATUS_OK);
 133                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 134         }
 135         last_status = status;
 136 }
 137
 138 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 139                         void *recvCallBackParam)
 140 {
 141         UDE_hw_status_t status = STATUS_FAILED;
 142         static UDE_hw_status_t last_status;
 143         static struct timespec last_sent;
 144         //struct camera_result_type *instance = (struct camera_result_type *)vinstance;
 145         switch (info->status) {
 146                 case NEW_DATA:
 147                         status = STATUS_OK;
 148                         break;
 149                 case DEADLINE:
 150                         status = STATUS_FAILED;
 151                         break;
 152         }
 153         if (status != last_status ||
 154             miliseconds_since(&last_sent) > 1000) {
 155                 uoled_display_status(CAM, STATUS_OK);
 156                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 157         }
 158         last_status = status;
 159 }
 160
 161 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 162                         void *recvCallBackParam)
 163 {
 164         UDE_hw_status_t status = STATUS_FAILED;
 165         static UDE_hw_status_t last_status;
 166         static struct timespec last_sent;
 167         //struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 168         switch (info->status) {
 169                 case NEW_DATA:
 170                         status = STATUS_OK;
 171                         break;
 172                 case DEADLINE:
 173                         status = STATUS_FAILED;
 174                         break;
 175         }
 176         if (status != last_status ||
 177             miliseconds_since(&last_sent) > 1000) {
 178                 uoled_display_status(HOK, STATUS_OK);
 179                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 180         }
 181         last_status = status;
 182 }
 183
 184 void rcv_ref_pos_cb (const ORTERecvInfo *info, void *vinstance,
 185                         void *recvCallBackParam)
 186 {
 187         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 188         switch (info->status) {
 189                 case NEW_DATA:
 190                         uoled_display_position(&orte_data->ref_pos);
 191                         break;
 192                 case DEADLINE:
 193                         break;
 194         }
 195 }
 196
 197 void rcv_est_pos_odo_cb (const ORTERecvInfo *info, void *vinstance,
 198                         void *recvCallBackParam)
 199 {
 200         UDE_hw_status_t status = STATUS_FAILED;
 201         static UDE_hw_status_t last_status;
 202         static struct timespec last_sent;
 203         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 204         switch (info->status) {
 205                 case NEW_DATA:
 206                         uoled_display_position(&orte_data->est_pos_odo);
 207                         status = STATUS_OK;
 208                         break;
 209                 case DEADLINE:
 210                         status = STATUS_FAILED;
 211                         break;
 212         }
 213         if (status != last_status ||
 214             miliseconds_since(&last_sent) > 1000) {
 215                 uoled_display_status(ODO, STATUS_OK);
 216                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 217         }
 218         last_status = status;
 219 }
 220
 221 void rcv_fsm_main_cb(const ORTERecvInfo *info, void *vinstance,
 222                        void *recvCallBackParam)
 223 {
 224         char status[MGS_LENGTH_DISPLAY_SM];
 225         static char last_status[MGS_LENGTH_DISPLAY_SM];
 226         static struct timespec last_sent;
 227         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 228         switch (info->status) {
 229                 case NEW_DATA:
 230                         strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
 231                         status[MGS_LENGTH_DISPLAY_SM-1]='\0';
 232                         //uoled_display_fsm(FSM_MAIN, fsm_state->state_name);
 233                         break;
 234                 case DEADLINE:
 235                         strcpy(status, "?");
 236                         //uoled_display_fsm(FSM_MAIN, "?");
 237                         break;
 238         }
 239         if (strcmp(status,last_status) != 0 ||
 240             miliseconds_since(&last_sent) > 1000) {
 241                 uoled_display_fsm(FSM_MAIN, status);
 242                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 243         }
 244         strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
 245 }
 246
 247 void rcv_fsm_act_cb(const ORTERecvInfo *info, void *vinstance,
 248                      void *recvCallBackParam)
 249 {
 250         char status[MGS_LENGTH_DISPLAY_SM];
 251         static char last_status[MGS_LENGTH_DISPLAY_SM];
 252         static struct timespec last_sent;
 253         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 254         switch (info->status) {
 255                 case NEW_DATA:
 256                         strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
 257                         status[MGS_LENGTH_DISPLAY_SM-1]='\0';
 258                         break;
 259                 case DEADLINE:
 260                         strcpy(status, "?");
 261                         break;
 262         }
 263         if (strcmp(status,last_status) != 0 ||
 264             miliseconds_since(&last_sent) > 1000) {
 265                 uoled_display_fsm(FSM_ACT, status);
 266                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 267         }
 268         strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
 269 }
 270
 271 void rcv_fsm_motion_cb(const ORTERecvInfo *info, void *vinstance,
 272                      void *recvCallBackParam)
 273 {
 274         char status[MGS_LENGTH_DISPLAY_SM];
 275         static char last_status[MGS_LENGTH_DISPLAY_SM];
 276         static struct timespec last_sent;
 277         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 278         switch (info->status) {
 279                 case NEW_DATA:
 280                         strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
 281                         status[MGS_LENGTH_DISPLAY_SM-1]='\0';
 282                         break;
 283                 case DEADLINE:
 284                         strcpy(status, "?");
 285                         break;
 286         }
 287         if (strcmp(status,last_status) != 0 ||
 288             miliseconds_since(&last_sent) > 1000) {
 289                 uoled_display_fsm(FSM_MOVE, status);
 290                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 291         }
 292         strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
 293 }
 294
 295
 296
 297
 298 int main(int argc, char *argv[])
 299 {
 300         //fd_set read_fd_set;
 301         //struct timeval timeout;
 302         int ret;
 303         //int size;
 304
 305         struct robottype_orte_data orte;
 306         char * tty;
 307
 308         //struct can_frame frame;
 309         tty = getenv("DISPLAY_TTY");
 310         if (!tty){
 311                 tty="/dev/ttyUSB0";
 312                 printf("displayd: enviroment variable 'DISPLAY_TTY' was not set, using default value '%s'.\n", tty);
 313         }
 314
 315         sercom = uoled_init(tty, NULL);
 316         if (strcmp(tty, "/dev/null") != 0) {
 317         // :wq
 318                 printf("displayd: init serial communication OK\n");
 319         } else {
 320                 printf("displayd: init serial communication failed\n");
 321                 //return 1;
 322         }
 323
 324         orte.strength = 1;
 325
 326         /* orte initialization */
 327         if(robottype_roboorte_init(&orte)) {
 328                 printf("Roboorte initialization failed! Exiting...\n");
 329                 return -1;
 330         }
 331         else
 332                 printf("Roboorte OK\n");
 333
 334         /* creating publishers */
 335         //robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 336         //robottype_publisher_motion_status_create(&orte, NULL, NULL);
 337         //robottype_publisher_odo_data_create(&orte, NULL, NULL);
 338         //robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 339         //robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 340         //robottype_publisher_corr_trig_create(&orte, NULL, NULL);
 341         //robottype_publisher_corr_distances_create(&orte, NULL, NULL);
 342         //robottype_publisher_actuator_status_create(&orte, NULL, NULL);
 343         //printf("Publishers OK\n");
 344
 345         /* creating subscribers */
 346         robottype_subscriber_odo_data_create(&orte, rcv_odo_data_cb, &orte);
 347         robottype_subscriber_motion_irc_create(&orte, rcv_motion_irc_cb, &orte);
 348         robottype_subscriber_pwr_voltage_create(&orte, rcv_pwr_voltage_cb, &orte);
 349         robottype_subscriber_est_pos_odo_create(&orte, rcv_est_pos_odo_cb, &orte);
 350         robottype_subscriber_ref_pos_create(&orte, rcv_ref_pos_cb, &orte);
 351         //robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
 352         //robottype_subscriber_motion_speed_create(&orte,       rcv_motion_speed_cb, &orte);
 353         //robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
 354         robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
 355         robottype_subscriber_camera_result_create(&orte, rcv_camera_result_cb, &orte);
 356         robottype_subscriber_fsm_main_create(&orte, rcv_fsm_main_cb, &orte);
 357         robottype_subscriber_fsm_act_create(&orte, rcv_fsm_act_cb, &orte);
 358         robottype_subscriber_fsm_motion_create(&orte, rcv_fsm_motion_cb, &orte);
 359
 360         printf("subscribers created OK\n");
 361
 362         struct pollfd pfd[1] = {{
 363                         .fd = sercom->fd,
 364                         .events = POLLIN,
 365                 }};
 366
 367         /* main loop */
 368         for(;;)
 369         {
 370                 if (strcmp(tty, "/dev/null") != 0) {
 371                         ret = poll(pfd, 1, 500/*ms*/);
 372                         if (ret > 0) {
 373 #warning TODO: process received serial data here
 374                                 // nejeka_fce_co_obstara_ktery_publisher_se_zavola = zfce...
 375                                 // zfce(uoled_recieve_cmd());
 376                         }
 377                 } else {
 378                         usleep(500/*ms*/*1000);
 379                         ret = 0;
 380                 }
 381                 if (ret == 0)
 382                         uoled_display_alive();
 383         }
 384         return 0;
 385 }