Go to the documentation of this file.00001
00002 #include <stdlib.h>
00003 #include <string.h>
00004 #include <termios.h>
00005 #include <unistd.h>
00006 #include <fcntl.h>
00007 #include <sys/ioctl.h>
00008 #include <sys/types.h>
00009 #include <sys/stat.h>
00010 #include <stdbool.h>
00011
00012 #include <time.h>
00013 #include <sys/time.h>
00014
00015 #include <sched.h>
00016 #include <pthread.h>
00017
00018 static int portFd = -1;
00019 bool is_busy = false;
00020
00021 int portOpen (char *device, int baud)
00022 {
00023 struct termios options;
00024 speed_t myBaud;
00025 int status;
00026
00027 switch (baud)
00028 {
00029 case 50: myBaud = B50 ; break;
00030 case 75: myBaud = B75 ; break;
00031 case 110: myBaud = B110 ; break;
00032 case 134: myBaud = B134 ; break;
00033 case 150: myBaud = B150 ; break;
00034 case 200: myBaud = B200 ; break;
00035 case 300: myBaud = B300 ; break;
00036 case 600: myBaud = B600 ; break;
00037 case 1200: myBaud = B1200 ; break;
00038 case 1800: myBaud = B1800 ; break;
00039 case 2400: myBaud = B2400 ; break;
00040 case 9600: myBaud = B9600 ; break;
00041 case 19200: myBaud = B19200 ; break;
00042 case 38400: myBaud = B38400 ; break;
00043 case 57600: myBaud = B57600 ; break;
00044 case 115200: myBaud = B115200 ; break;
00045 case 230400: myBaud = B230400 ; break;
00046
00047 default:
00048 return -2 ;
00049 }
00050
00051 if ((portFd = open (device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK)) == -1)
00052 return -1;
00053
00054 fcntl (portFd, F_SETFL, O_RDWR);
00055
00056
00057
00058 tcgetattr (portFd, &options);
00059
00060 cfmakeraw (&options);
00061 cfsetispeed (&options, myBaud);
00062 cfsetospeed (&options, myBaud);
00063
00064 options.c_cflag |= (CLOCAL | CREAD);
00065 options.c_cflag &= ~PARENB;
00066 options.c_cflag &= ~CSTOPB;
00067 options.c_cflag &= ~CSIZE;
00068 options.c_cflag |= CS8;
00069 options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
00070 options.c_oflag &= ~OPOST;
00071
00072 options.c_cc [VMIN] = 0;
00073 options.c_cc [VTIME] = 100;
00074
00075 tcsetattr (portFd, TCSANOW | TCSAFLUSH, &options);
00076
00077 ioctl (portFd, TIOCMGET, &status);
00078
00079 status |= TIOCM_DTR;
00080 status |= TIOCM_RTS;
00081
00082 ioctl (portFd, TIOCMSET, &status);
00083
00084 usleep (10000);
00085
00086 return portFd;
00087 }
00088
00089 void portFlush (void)
00090 {
00091 tcflush (portFd, TCIOFLUSH);
00092 }
00093
00094 void portClose (void)
00095 {
00096 close (portFd);
00097 }
00098
00099
00100 int portDataAvail (void)
00101 {
00102 int result;
00103
00104 if (ioctl (portFd, FIONREAD, &result) == -1)
00105 return -1;
00106
00107 return result;
00108 }
00109
00110 int portGetchar (void)
00111 {
00112 unsigned char x;
00113
00114 if (portDataAvail ())
00115 {
00116 if (read (portFd, &x, 1) == 1)
00117 return ((int)x) & 0xFF;
00118
00119 return -1;
00120 }
00121
00122 return -1;
00123 }
00124
00125
00126 void portPutchar (int data)
00127 {
00128 unsigned char c = (unsigned char)data;
00129 write (portFd, &c, 1);
00130 }
00131
00132 void sendCmd(char cmd, char data1, char data2)
00133 {
00134 while(is_busy)
00135 usleep(1000);
00136
00137 is_busy = true;
00138 char data[4] = {'$', cmd, data1, data2};
00139 write(portFd, &data, 4);
00140 is_busy = false;
00141 }
00142
00143 void lockSerial(bool lock)
00144 {
00145 is_busy = lock;
00146 }
00147
00148 bool getSerial(void)
00149 {
00150 return is_busy;
00151 }
00152