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