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
  21
  22 #ifdef CONFIG_DEBUG_DISPLAYD
  23 /** Used to control verbosity */
  24 #define DEBUG
  25 #endif
  26
  27 //#define DEBUG
  28
  29 struct sercom_data* sercom;
  30
  31 void rcv_pwr_voltage_cb (const ORTERecvInfo *info, void *vinstance,
  32                         void *recvCallBackParam)
  33 {
  34         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
  35         switch (info->status) {
  36                 case NEW_DATA:
  37                         uoled_display_voltage(&orte_data->pwr_voltage);
  38                         break;
  39                 case DEADLINE:
  40                         printf("ORTE deadline occurred - PWR_VOLTAGE receive\n");
  41                         break;
  42         }
  43 }
  44
  45 void rcv_ref_pos_cb (const ORTERecvInfo *info, void *vinstance,
  46                         void *recvCallBackParam)
  47 {
  48         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
  49         switch (info->status) {
  50                 case NEW_DATA:
  51                         uoled_display_position(&orte_data->ref_pos);
  52                         break;
  53                 case DEADLINE:
  54                         printf("ORTE deadline occurred - REF_POS receive\n");
  55                         break;
  56         }
  57 }
  58
  59 void rcv_est_pos_odo_cb (const ORTERecvInfo *info, void *vinstance,
  60                         void *recvCallBackParam)
  61 {
  62         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
  63         switch (info->status) {
  64                 case NEW_DATA:
  65                         uoled_display_position(&orte_data->est_pos_odo);
  66                         break;
  67                 case DEADLINE:
  68                         printf("ORTE deadline occurred - EST_POS_ODO receive\n");
  69                         break;
  70         }
  71 }
  72
  73 int main(int argc, char *argv[])
  74 {
  75         //fd_set read_fd_set;
  76         //struct timeval timeout;
  77         //int ret;
  78         //int size;
  79
  80         struct robottype_orte_data orte;
  81         char * tty;
  82         sigset_t sigset;
  83         int sig;
  84
  85         //struct can_frame frame;
  86         tty = getenv("DISPLAY_TTY");
  87         if (!tty)
  88                 tty="/dev/ttyUSB0";
  89
  90         sercom = uoled_init(tty, NULL);
  91         if (strcmp(tty, "/dev/null") != 0) {
  92         // :wq
  93                 printf("displayd: init serial communication OK\n");
  94         } else {
  95                 printf("displayd: init serial communication failed\n");
  96                 //return 1;
  97         }
  98
  99         orte.strength = 1;
 100
 101         /* orte initialization */
 102         if(robottype_roboorte_init(&orte)) {
 103                 printf("Roboorte initialization failed! Exiting...\n");
 104                 return -1;
 105         }
 106         else
 107                 printf("Roboorte OK\n");
 108
 109         /* creating publishers */
 110         //robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 111         //robottype_publisher_motion_status_create(&orte, NULL, NULL);
 112         //robottype_publisher_odo_data_create(&orte, NULL, NULL);
 113         //robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 114         //robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 115         //robottype_publisher_corr_trig_create(&orte, NULL, NULL);
 116         //robottype_publisher_corr_distances_create(&orte, NULL, NULL);
 117         //robottype_publisher_actuator_status_create(&orte, NULL, NULL);
 118         //printf("Publishers OK\n");
 119
 120         /* creating subscribers */
 121         robottype_subscriber_pwr_voltage_create(&orte, rcv_pwr_voltage_cb, &orte);
 122         robottype_subscriber_est_pos_odo_create(&orte, rcv_est_pos_odo_cb, &orte);
 123         robottype_subscriber_ref_pos_create(&orte, rcv_ref_pos_cb, &orte);
 124         //robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
 125         //robottype_subscriber_motion_speed_create(&orte,       rcv_motion_speed_cb, &orte);
 126         //robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
 127         //robottype_subscriber_hokuyo_pitch_create(&orte, rcv_hokuyo_pitch_cb, &orte);
 128
 129         printf("subscribers created OK\n");
 130
 131         /* main loop */
 132         for(;;)
 133         {
 134                 if (strcmp(tty, "/dev/null") != 0) {
 135                         sigfillset(&sigset);
 136                         sigwait(&sigset, &sig);
 137                         break;
 138                 } else {
 139                   // spracovani serioveho portu, pri prijmu dat...
 140                 }
 141         }
 142         return 0;
 143 }