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 struct sercom_data* sercom;
  23
  24 void rcv_pwr_voltage_cb (const ORTERecvInfo *info, void *vinstance,
  25                         void *recvCallBackParam)
  26 {
  27         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
  28         switch (info->status) {
  29                 case NEW_DATA:
  30                         uoled_display_voltage(&orte_data->pwr_voltage);
  31                         uoled_display_status(PWR, STATUS_OK);
  32                         break;
  33                 case DEADLINE:
  34                         printf("ORTE deadline occurred - PWR_VOLTAGE\n");
  35                         uoled_display_status(PWR, STATUS_FAILED);
  36                         break;
  37         }
  38 }
  39
  40 void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
  41                         void *recvCallBackParam)
  42 {
  43         //struct odo_data_type *instance = (struct odo_data_type *)vinstance;
  44         switch (info->status) {
  45                 case NEW_DATA:
  46                         uoled_display_status(HOK, STATUS_OK);
  47                         break;
  48                 case DEADLINE:
  49                         printf("ORTE deadline occurred - ODO_DATA\n");
  50                         uoled_display_status(HOK, STATUS_FAILED);
  51                         break;
  52         }
  53 }
  54
  55 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
  56                         void *recvCallBackParam)
  57 {
  58         //struct motion_irc_type*instance = (struct motion_irc_type*)vinstance;
  59         switch (info->status) {
  60                 case NEW_DATA:
  61                         uoled_display_status(MOT, STATUS_OK);
  62                         break;
  63                 case DEADLINE:
  64                         printf("ORTE deadline occurred - MOTION_IRC\n");
  65                         uoled_display_status(MOT, STATUS_FAILED);
  66                         break;
  67         }
  68 }
  69
  70 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
  71                         void *recvCallBackParam)
  72 {
  73         //struct camera_result_type *instance = (struct camera_result_type *)vinstance;
  74         switch (info->status) {
  75                 case NEW_DATA:
  76                         uoled_display_status(CAM, STATUS_OK);
  77                         break;
  78                 case DEADLINE:
  79                         printf("ORTE deadline occurred - CAMERA_RESULT\n");
  80                         uoled_display_status(CAM, STATUS_FAILED);
  81                         break;
  82         }
  83 }
  84
  85 void rcv_ref_pos_cb (const ORTERecvInfo *info, void *vinstance,
  86                         void *recvCallBackParam)
  87 {
  88         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
  89         switch (info->status) {
  90                 case NEW_DATA:
  91                         uoled_display_position(&orte_data->ref_pos);
  92                         break;
  93                 case DEADLINE:
  94                         printf("ORTE deadline occurred - REF_POS\n");
  95                         break;
  96         }
  97 }
  98
  99 void rcv_est_pos_odo_cb (const ORTERecvInfo *info, void *vinstance,
 100                         void *recvCallBackParam)
 101 {
 102         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 103         switch (info->status) {
 104                 case NEW_DATA:
 105                         uoled_display_position(&orte_data->est_pos_odo);
 106                         uoled_display_status(ODO, STATUS_FAILED);
 107                         break;
 108                 case DEADLINE:
 109                         printf("ORTE deadline occurred - EST_POS_ODO\n");
 110                         uoled_display_status(ODO, STATUS_FAILED);
 111                         break;
 112         }
 113 }
 114
 115 int main(int argc, char *argv[])
 116 {
 117         //fd_set read_fd_set;
 118         //struct timeval timeout;
 119         //int ret;
 120         //int size;
 121
 122         struct robottype_orte_data orte;
 123         char * tty;
 124         sigset_t sigset;
 125         int sig;
 126
 127         //struct can_frame frame;
 128         tty = getenv("DISPLAY_TTY");
 129         if (!tty){
 130                 tty="/dev/ttyUSB0";
 131                 printf("displayd: enviroment variable 'DISPLAY_TTY' was not set, using default value '%s'.\n", tty);
 132         }
 133
 134         sercom = uoled_init(tty, NULL);
 135         if (strcmp(tty, "/dev/null") != 0) {
 136         // :wq
 137                 printf("displayd: init serial communication OK\n");
 138         } else {
 139                 printf("displayd: init serial communication failed\n");
 140                 //return 1;
 141         }
 142
 143         orte.strength = 1;
 144
 145         /* orte initialization */
 146         if(robottype_roboorte_init(&orte)) {
 147                 printf("Roboorte initialization failed! Exiting...\n");
 148                 return -1;
 149         }
 150         else
 151                 printf("Roboorte OK\n");
 152
 153         /* creating publishers */
 154         //robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 155         //robottype_publisher_motion_status_create(&orte, NULL, NULL);
 156         //robottype_publisher_odo_data_create(&orte, NULL, NULL);
 157         //robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 158         //robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 159         //robottype_publisher_corr_trig_create(&orte, NULL, NULL);
 160         //robottype_publisher_corr_distances_create(&orte, NULL, NULL);
 161         //robottype_publisher_actuator_status_create(&orte, NULL, NULL);
 162         //printf("Publishers OK\n");
 163
 164         /* creating subscribers */
 165         robottype_subscriber_odo_data_create(&orte, rcv_odo_data_cb, &orte);
 166         robottype_subscriber_motion_irc_create(&orte, rcv_motion_irc_cb, &orte);
 167         robottype_subscriber_pwr_voltage_create(&orte, rcv_pwr_voltage_cb, &orte);
 168         robottype_subscriber_est_pos_odo_create(&orte, rcv_est_pos_odo_cb, &orte);
 169         robottype_subscriber_ref_pos_create(&orte, rcv_ref_pos_cb, &orte);
 170         //robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
 171         //robottype_subscriber_motion_speed_create(&orte,       rcv_motion_speed_cb, &orte);
 172         //robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
 173         //robottype_subscriber_hokuyo_pitch_create(&orte, rcv_hokuyo_pitch_cb, &orte);
 174         robottype_subscriber_camera_result_create(&orte, rcv_camera_result_cb, &orte);
 175
 176         printf("subscribers created OK\n");
 177
 178         /* main loop */
 179         for(;;)
 180         {
 181                 if (strcmp(tty, "/dev/null") != 0) {
 182                         sigfillset(&sigset);
 183                         sigwait(&sigset, &sig);
 184                         break;
 185                 } else {
 186                   // spracovani serioveho portu, pri prijmu dat...
 187                 }
 188         }
 189         return 0;
 190 }