src/displayd/uoled.c

   1 /**
   2  * @file fsm.c
   3  * @author Martin Zidek
   4  *
   5  * @brief Library for communication with uOLED display using serial interface.
   6  */
   7
   8 #include <sercom.h>
   9
  10 #include <string.h>
  11 #include <fcntl.h>
  12 #include <signal.h>
  13 #include <unistd.h>
  14 #include <stdlib.h>
  15 #include <stdio.h>
  16 #include <robottype.h>
  17 #include <robottype.h>
  18 #include "uoled.h"
  19 #include <pthread.h>
  20 #include <robomath.h>
  21
  22 static struct sercom_data sercom;
  23
  24 static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
  25
  26 struct sercom_data* uoled_sercom_init(char * tty, void(*sighandler)(int))
  27 {
  28         int ret;
  29         strcpy((char *)&sercom.devname, tty);
  30         sercom.baudrate = 115200;
  31         sercom.parity = SERCOM_PARNONE;
  32         sercom.databits = 8;
  33         sercom.stopbits = 1;
  34         sercom.mode = 0;
  35         sercom.sighandler = NULL;
  36         ret = sercom_open(&sercom);
  37         if (ret < 0)
  38                 return NULL;
  39         else
  40                 return &sercom;
  41 }
  42
  43 void uoled_sercom_close()
  44 {
  45         if(sercom.mode != 0 && sercom.fd >=0 )
  46                 sercom_close(&sercom);
  47 }
  48
  49 int uoled_write_cmd(uint8_t *buff, int size)
  50 {
  51         int ret;
  52         int i,j;
  53         pthread_mutex_lock(&mutex);
  54         printf("sending command (%2d bytes): ", size);
  55         if (buff[0] == 0x81){
  56                 /* specialni vypis pro stav komponent */
  57                 printf(" 0x%02x ", buff[0]);
  58                 printf("(b) ");
  59                 for(i=1; i<size; i++){
  60                         for(j=sizeof(buff[i])*8; j >=0 ; j--)
  61                                 printf("%i",(buff[i] >> j) & 1 );
  62                         printf(" (component: %i, status: %i)",buff[i] & COMPONENTS_MASK, (buff[i] & STATUS_MASK) >> STATUS_SHIFT);
  63                 }
  64         } else {
  65                 for(i=0; i<size; i++)
  66                 if (buff[i] < ' ' || buff[i] > '~' || buff[0] == 0x81)
  67                                 printf(" 0x%02x ", buff[i]);
  68                         else
  69                                 printf("%c", buff[i]);
  70         }
  71         printf("\n");
  72
  73         ret = write(sercom.fd, buff, size);
  74         pthread_mutex_unlock(&mutex);
  75         if(ret != size) {
  76                 printf("uoled: WRITE FAILED!\n");
  77                 return -1;
  78         }
  79
  80         return 0;
  81 }
  82
  83 int uoled_read_cmd(uint8_t *buff, int size)
  84 {
  85         int ret;
  86         pthread_mutex_lock(&mutex);
  87         ret = read(sercom.fd, buff, size);
  88         pthread_mutex_unlock(&mutex);
  89         if(ret != size) {
  90                 printf("uoled: READ FAILED!\n");
  91                 return -1;
  92         }
  93         // jinak vracim prvni
  94         return ret;
  95 }
  96
  97 int uoled_recieve_cmd(UDE_recieve_cmd_t *cmd)
  98 {
  99         return uoled_read_cmd(&cmd, 1);
 100 }
 101
 102 int uoled_display_status(UDE_component_t c, UDE_hw_status_t s)
 103 {
 104         int ret;
 105         uint8_t msg[MGS_LENGTH_DISPLAY_STATUS];
 106
 107         msg[0]=ID_DISPLAY_STATUS;
 108         msg[1]=s << 5;
 109         msg[1]+=c;
 110
 111         ret = uoled_write_cmd(msg, MGS_LENGTH_DISPLAY_STATUS);
 112         if(ret)
 113                 return -1;
 114
 115         return 0;
 116 }
 117
 118 int uoled_display_fsm(UDE_fsm_t fsm, char * state)
 119 {
 120         int ret, i;
 121         uint8_t msg[MGS_LENGTH_DISPLAY_SM];
 122
 123         msg[0]=ID_DISPLAY_FSM + fsm;
 124         for( i = 0; i < MGS_LENGTH_DISPLAY_SM - 2; i++){
 125                 msg[i+1]=state[i];
 126                 if(state[i] == '\0')
 127                         break;
 128         }
 129         msg[MGS_LENGTH_DISPLAY_SM - 1] = '\0';
 130
 131         ret = uoled_write_cmd(msg, i+2);
 132         if(ret)
 133                 return -1;
 134
 135         return 0;
 136 }
 137
 138 int uoled_display_alive()
 139 {
 140         int ret;
 141         uint8_t msg[MGS_LENGTH_DISPLAY_ALIVE];
 142         msg[0]=ID_DISPLAY_ALIVE;
 143
 144         ret = uoled_write_cmd(msg, MGS_LENGTH_DISPLAY_ALIVE);
 145         if(ret)
 146                 return -1;
 147
 148         return 0;
 149 }
 150
 151 int uoled_display_color(int color)
 152 {
 153         int ret;
 154         uint8_t msg[MGS_LENGTH_DISPLAY_COLOR];
 155         msg[0]=ID_DISPLAY_COLOR;
 156         msg[1]=(char)color;
 157
 158         ret = uoled_write_cmd(msg, MGS_LENGTH_DISPLAY_COLOR);
 159         if(ret)
 160                 return -1;
 161
 162         return 0;
 163 }
 164
 165 int uoled_display_voltage(double voltage33, double voltage50,
 166                         double voltage80, double voltageBAT)
 167 {
 168         int ret;
 169         uint8_t msg[MGS_LENGTH_DISPLAY_VOLTAGE];
 170         unsigned char status_indicator = 0;
 171
 172         if( voltageBAT < WARNING_VOLTAGEBAT)
 173                 status_indicator += 1 << 4;
 174         if( voltage33 < TRESHOLDS_VOLTAGE33 )
 175                 status_indicator += 1 << 3;
 176         if( voltage50 < TRESHOLDS_VOLTAGE50 )
 177                 status_indicator += 1 << 2;
 178         if( voltage80 < TRESHOLDS_VOLTAGE80 )
 179                 status_indicator += 1 << 1;
 180         if( voltageBAT < TRESHOLDS_VOLTAGEBAT )
 181                 status_indicator += 1 << 0;
 182
 183         msg[0]=ID_DISPLAY_VOLTAGE;
 184         msg[1]=(uint8_t)status_indicator;
 185
 186         sprintf((char*)(msg+2), "%2i%i%i", (int)voltage33 % 100,
 187                 (int)(voltage33 * 10) % 10, (int)(voltage33 * 100) % 10);
 188         sprintf((char*)(msg+6), "%2i%i%i", (int)voltage50 % 100,
 189                 (int)(voltage50 * 10) % 10, (int)(voltage50 * 100) % 10);
 190         sprintf((char*)(msg+10), "%2i%i%i", (int)voltage80 % 100,
 191                 (int)(voltage80 * 10) % 10, (int)(voltage80 * 100) % 10);
 192         sprintf((char*)(msg+14), "%2i%i%i", (int)voltageBAT % 100,
 193                 (int)(voltageBAT * 10) % 10, (int)(voltageBAT * 100) % 10);
 194
 195         ret = uoled_write_cmd(msg, MGS_LENGTH_DISPLAY_VOLTAGE);
 196         if(ret)
 197                 return -1;
 198
 199         return 0;
 200 }
 201
 202 int uoled_display_position(double x, double y, double phi)
 203 {
 204         int ret;
 205         uint8_t msg[MGS_LENGTH_DISPLAY_POSITION];
 206
 207         //printf(" x: %f \n y: %f \n phi: %f\n",x,y,phi);
 208
 209         msg[0]=ID_DISPLAY_POSITION;
 210         if ( x >= 100 )
 211                 sprintf((char*)(msg+1),"%3i", (int)x % 1000);
 212         else
 213                 if ( x >= 10 )
 214                         sprintf((char*)(msg+1),"%2i%i", (int)x % 100,
 215                                 (int)(x*10) % 10);
 216                 else
 217                         sprintf((char*)(msg+1),"%i%i%i", (int)x % 10,
 218                                 (int)(x*10) % 10, (int)(x*100) % 10);
 219         if ( y >= 100 )
 220                 sprintf((char*)(msg+4),"%3i", (int)y % 1000);
 221         else
 222                 if ( y >= 10 )
 223                         sprintf((char*)(msg+4),"%2i%i", (int)y % 100,
 224                                 (int)(y*10) % 10);
 225                 else
 226                         sprintf((char*)(msg+4),"%i%i%i", (int)y % 10,
 227                                 (int)(y*10) % 10, (int)(y*100) % 10);
 228
 229         phi = RAD2DEG(phi);
 230         phi = fmod(phi, 360);
 231         if ( phi < 0 )
 232                 phi += 360;
 233
 234         sprintf((char*)(msg+7),"%3i", (int)phi);
 235
 236         ret = uoled_write_cmd(msg, MGS_LENGTH_DISPLAY_POSITION);
 237         if(ret)
 238                 return -1;
 239
 240         return 0;
 241 }