source: trunk/shared_serial/src/LxSerial.cpp

Last change on this file was 49, checked in by wcaarls, 11 years ago

Reinstated rosbuild shared_serial (catkin version is now on github)

File size: 13.5 KB
Line 
1//============================================================================
2// Name        : LxSerial.cpp
3// Author      : Eelko van Breda,www.dbl.tudelft.nl
4// Version     : 0.1
5// Copyright   : Copyright (c) 2008 LGPL
6// Description : serial communicatin class linux
7//============================================================================
8
9#include <stdio.h>
10#include <stdlib.h>
11#include <unistd.h>
12#include <string.h>
13#include <sys/types.h>
14#include <sys/socket.h>
15#include <netinet/in.h>
16#include <netdb.h>
17#include <poll.h>
18#include <endian.h>
19
20#include "shared_serial/LxSerial.h"
21
22
23/*      constructor */
24LxSerial::LxSerial()
25{
26        hPort                   = INVALID_DEVICE_HANDLE;
27//      b_initialised   = false;
28        b_clear_echo    = false;
29        b_hw_flow_control = false;
30        b_socket = false;
31}
32
33/* return name of the port that was opened */
34std::string& LxSerial::get_port_name()
35{
36        return s_port_name;
37}
38
39void LxSerial::set_port_type(LxSerial::PortType port_type)
40{
41        switch (port_type)
42        {
43                case RS232:
44                        b_rts                           =       false;
45                        break;
46
47                case RS485_FTDI:
48                        b_rts                           =       false;
49                        break;
50
51                case RS485_EXAR:
52                        b_rts                           =       true;
53                        b_clear_echo            =       true;
54                        break;
55
56                case RS485_SMSC:
57                        b_rts                           =       false;
58                        b_clear_echo            =       true;
59                        break;
60                       
61                case TCP:
62                        b_socket = true;
63                        b_rts = false;
64                        b_clear_echo = false;
65                        break;
66
67                default:
68                        perror("LxSerial: no port type specified");
69                        break;
70        }
71}
72
73/* open port */
74bool LxSerial::port_open(const std::string& portname, LxSerial::PortType port_type)
75{
76        // Set port type
77        set_port_type(port_type);
78       
79        if (b_socket)
80        {
81                int optval = 1;
82
83                struct sockaddr_in addr;
84                struct hostent *server;
85
86                hPort = socket(AF_INET, SOCK_STREAM, 0);
87                setsockopt(hPort, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof optval);
88
89                bzero((char *) &addr, sizeof(addr));
90                addr.sin_family = AF_INET;
91
92                std::string host_name   = portname.substr(0, portname.find(':'));
93                std::string port_number = portname.substr(portname.find(':')+1);
94
95                server = gethostbyname(host_name.c_str());
96                if (!server)
97                {
98                        perror("unknown remote serial host name");
99                        return false;
100                }
101
102                bcopy((char *)server->h_addr, (char *)&addr.sin_addr.s_addr, server->h_length);
103                addr.sin_port = htons(atoi(port_number.c_str()));
104
105                if (connect(hPort,(struct sockaddr *) &addr,sizeof(addr)) < 0)
106                {
107                        perror("error connecting to remote serial host");
108                        return false;
109                }
110        }
111        else
112        {
113                // Open port
114                hPort = open(portname.c_str(), O_RDWR | O_NOCTTY);//|O_NDELAY);                                         // open the serial device
115                                                                                                                                                                // O_RDWR = open read an write
116                                                                                                                                                                // The O_NOCTTY flag tells UNIX that this program doesn't want to be the "controlling terminal" for that 
117                                                                                                                                                                // port. If you don't specify this then any input (such as keyboard abort signals and so forth) will affect 
118                                                                                                                                                                // your process. Programs like getty(1M/8) use this feature when starting the login process, but normally a 
119                                                                                                                                                                // user program does not want this behavior.
120                                                                                                                                                                // The O_NDELAY flag tells UNIX that this program doesn't care what state the DCD signal line is in ­ 
121                                                                                                                                                                // whether the other end of the port is up and running. If you do not specify this flag, your process will be 
122                                                                                                                                                                // put to sleep until the DCD signal line is the space voltage.
123
124                if (hPort < 0) {                                                                                                                        //check if port opened correctly
125                        perror(" Could not open serial port, aborting");
126                        return false;
127                }
128
129
130                tcgetattr(hPort, &options);                                                                     // get the current termios (com port options struct) from the kernel
131                tcgetattr(hPort, &old_options);                                                                 // get the current termios copy to restore on close
132
133                cfsetispeed(&options, B115200);                                                                 // set incomming baudrate to standard 9600, this can be changed with the function set_speed()
134                cfsetospeed(&options, B115200);                                                                 // set outgoing baudrate to standard 9600
135
136                                                                                                                                        // c_cflag Control Options
137                                                                                                                                        // |= (..) means enabled
138                                                                                                                                        // &= ~(..) means not enabled
139                options.c_cflag |= (CLOCAL|CREAD|CS8);                                                          // CREAD = enanble receiver
140                                                                                                                                        // CLOCAL = Local Line, do not change owner of port
141                                                                                                                                        // CS8 = 8 databits
142                options.c_cflag &= ~(CRTSCTS|PARENB|CSTOPB);                                    // Disable HW flow control
143                                                                                                                                        // no paritybit, PARENB = paritybit
144                                                                                                                                        // 1 stop bit, CSTOPB = 2 stop bits (1 otherwise)
145                if (b_hw_flow_control){
146                        options.c_cflag |= (CRTSCTS);                                                           // Enable HW flow control
147                }
148
149                                                                                                                                        // c_lflag Line Options ( controls how input charackters are handeled by the serial driver
150                options.c_lflag &= ~(ECHO|ECHONL|ECHOE|ICANON|ISIG|IEXTEN);
151//      options.c_lflag &= ~(ECHO|ECHONL|ICANON|ISIG|IEXTEN);
152                                                                                                                                        // ~(..) means not enabled
153                                                                                                                                        // ECHO echoing of input characters
154                                                                                                                                        // ECHOE echo erease character as BS-SP-BS
155                                                                                                                                        // ECHONL mimics echo but doesn't move to next line when it ends
156                                                                                                                                        // ICANON cononical input = line oriented input, else RAW
157                                                                                                                                        // ISIG ENABLE SIGINTR, SIGSUSP, SIGDSUSP and SIGQUIT signals
158                                                                                                                                        // IEXTERN = enable extended functions
159
160                                                                                                                                        // c_iflag Input Options
161                options.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON|IXOFF);
162//      options.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON);
163                                                                                                                                        // ~(..) means not enabled
164                                                                                                                                        // IGNBRK ignore break condition
165                                                                                                                                        // BRKINT send a SIGINT when a break condition is detected
166                                                                                                                                        // PARMRK Mark parity errors
167                                                                                                                                        // ISTRIP strip parity bits
168                                                                                                                                        // INLCR map new line to CR
169                                                                                                                                        // IGNCR ignore CR
170                                                                                                                                        // ICRNL map CR to new line
171                                                                                                                                        // IXON Enable software flow control (outgoing)
172                                                                                                                                        // IXOFF Enable software flow control (incomming)
173
174                                                                                                                                        // c_oflag output options
175                options.c_oflag &= ~(OPOST);
176                                                                                                                                        // ~(..) means not enabled
177                                                                                                                                        // OPOST postprocess output (if not set RAW output)
178                                                                                                                                        // when OPOST is disabled all other other option bits of c-Oflag are ignored
179
180                                                                                                                                        // c_cc control character array (control caracter options and timeouts
181                                                                                                                                        // Timeout 0.005 sec for first byte, read minimum of 0 bytes
182                                                                                                                                        // timeouts are ignored in cononical input or when NDELAY option is set.
183                options.c_cc[VMIN]     = 0;                                                                     // VMIN Minimum number of characters to read
184                options.c_cc[VTIME]    = WAIT_FOR_DATA_DSEC;                                    // Time to wait for data (tenths of seconds)
185
186//              fcntl(hPort, F_SETFL, FNDELAY); // don't wait for input when reading data
187
188                if (b_rts){                                                                                                             // if we are using RS_485_EXAR type, we clear the RTS signal
189                        int msc = TIOCM_RTS;
190                        ioctl(hPort, TIOCMBIC, &msc);                                                           // Clear RTS signal
191                        usleep(100);                                                                                            // WAIT FOR SIGNAL TO BE CLEARD
192                }
193
194
195
196                if (tcsetattr(hPort,TCSANOW, &options)!=0){                                             // Set the new options for the port now
197                        perror("Error: Could not set serial port settings");
198                        return false;
199                }
200
201                usleep(100);                                                                                                    // additional wait for correct functionality
202                tcflush(hPort, TCIOFLUSH);                                                                              // flush terminal data
203        }
204
205        // Save port name
206        s_port_name = portname;
207
208        return true;
209}
210
211bool    LxSerial::is_port_open()
212{
213        return hPort >= 0;
214}
215
216// speedcontrol */
217bool    LxSerial::set_speed( LxSerial::PortSpeed baudrate )
218{
219        if (b_socket)
220                return false;
221
222        cfsetispeed(&options, baudrate);                                                                //set incoming baud rate
223        cfsetospeed(&options, baudrate);                                                                //set outgoing baud rate
224
225        if (tcsetattr(hPort,TCSANOW, &options)!=0){                                             // Set the new options for the port now
226                perror("Error: Could not set serial port baudrate");
227                return false;
228        }
229        usleep(100);                                                                                                    // additional wait for correct functionality
230        tcflush(hPort, TCIOFLUSH);                                                                              // flush terminal data
231        return true;
232}
233
234bool LxSerial::set_speed_int(const int baudrate)
235{
236        LxSerial::PortSpeed baud;
237        // Baud rate conversion from index to real baud rate :(
238        switch (baudrate)
239        {
240                case 4000000:   baud = S4000000;        break;
241                case 3500000:   baud = S3500000;        break;
242                case 3000000:   baud = S3000000;        break;
243                case 2500000:   baud = S2500000;        break;
244                case 2000000:   baud = S2000000;        break;
245                case 1500000:   baud = S1500000;        break;
246                case 1152000:   baud = S1152000;        break;
247                case 1000000:   baud = S1000000;        break;
248                case 921600:    baud = S921600; break;
249                case 576000:    baud = S576000; break;
250                case 500000:    baud = S500000; break;
251                case 460800:    baud = S460800; break;
252                case 230400:    baud = S230400; break;
253                case 115200:    baud = S115200; break;
254                case 57600:             baud = S57600;  break;
255                case 38400:             baud = S38400;  break;
256                case 19200:             baud = S19200;  break;
257                case 9600:              baud = S9600;   break;
258                case 4800:              baud = S4800;   break;
259                case 2400:              baud = S2400;   break;
260                case 1800:              baud = S1800;   break;
261                case 1200:              baud = S1200;   break;
262                case 600:               baud = S600;    break;
263                case 300:               baud = S300;    break;
264                case 200:               baud = S200;    break;
265                case 150:               baud = S150;    break;
266                case 134:               baud = S134;    break;
267                case 110:               baud = S110;    break;
268                case 75:                baud = S75;             break;
269                case 50:                baud = S50;             break;
270                default:
271                        printf("This is not a legal portspeed!\n");
272                        return false;
273        }
274
275        set_speed(baud);
276        return true;
277}
278
279// clear echoed characters from input and detect collisions on write, use for EXAR RS485
280void    LxSerial::set_clear_echo(bool clear)
281{
282        b_clear_echo = clear;
283}
284
285// close the serial port
286bool    LxSerial::port_close()
287{
288        if (hPort==INVALID_DEVICE_HANDLE)
289                return true;
290
291        if (!b_socket)
292        {
293                if (tcsetattr(hPort,TCSANOW, &old_options)!=0) {                                // restore the old port settings
294                        perror("Warning: Could not restore serial port settings.");
295                }
296        }
297
298        if(close(hPort) == -1) {                                                                                //close serial port
299                perror("Error: Could not close serial port.");
300                return false;
301        }
302        hPort = INVALID_DEVICE_HANDLE;
303        return true;
304}
305
306int             LxSerial::port_read(unsigned char* buffer, int numBytes) const
307{
308        int nBytesRead = read(hPort, buffer, numBytes);
309
310        #ifdef __DBG__
311        printf("read  ");
312        for (int i=0;i<nBytesRead;i++)
313                { printf("%02X ",buffer[i]); }
314        printf("(%d)\n",nBytesRead);
315        #endif
316
317        return nBytesRead;
318}
319
320int     LxSerial::port_read(unsigned char* buffer, int numBytes, int seconds, int microseconds)
321{
322        // Init time variables (they are decreased by wait_for_input)
323        int s = seconds;
324        int us = microseconds;
325        int nBytesRead = 0;
326        while (nBytesRead < numBytes)
327        {
328                if( wait_for_input( &s, &us) )
329                {
330                        int partialRead = read(hPort, buffer + nBytesRead, numBytes - nBytesRead); // Read data
331                        nBytesRead += partialRead;
332                }
333                else if (nBytesRead < 0)
334                {
335                        #ifdef __DBG__
336                        printf("Read Timeout... \n");
337                        #endif
338                        return READ_ERROR;
339                }
340                else
341                        break;
342        }
343
344        #ifdef __DBG__
345        printf("read  ");
346        for (int i=0;i<nBytesRead;i++)
347                { printf("%02X ",buffer[i]); }
348        printf("(%d)\n",nBytesRead);
349        #endif
350
351        return nBytesRead;
352}
353
354bool    LxSerial::wait_for_input(int *seconds, int *microseconds)
355{
356        fd_set readset;
357        timeval timeout;
358        timeout.tv_sec = *seconds;                                                                              // seconds
359        timeout.tv_usec = *microseconds;                                                                        // microseconds
360        FD_ZERO(&readset);                                                                                      // clear file discriptor
361        FD_SET(hPort, &readset);                                                                                // set filediscripter for port
362        int res = select(hPort+1, &readset, NULL, NULL, &timeout);      // wait till readable data is in the buffer
363        *seconds = timeout.tv_sec;
364        *microseconds = timeout.tv_usec;
365        return res == 1;
366}
367
368int             LxSerial::port_write(unsigned char* buffer, int numBytes)
369{
370        int msc = TIOCM_RTS;
371        if (b_rts){                                                                                                             // control the RTS signal if needed
372                ioctl(hPort, TIOCMBIS, &msc);                                                           // before you write set RTS signal
373                usleep(1000);                                                                                           // wait until the RTS signal is high
374        }
375
376        int numBytesWritten = write(hPort, buffer, numBytes);                   // write data
377
378        if (numBytes != numBytesWritten){
379                perror("Error while writing to serial port");
380                assert(numBytes == numBytesWritten);
381        }
382
383        #ifdef __DBG__
384        printf("write ");
385        for (int i=0;i<numBytes;i++)
386                { printf("%02X ",buffer[i]); }
387        printf("(%d)",numBytesWritten);
388        printf("\n");
389        #endif
390       
391        tcdrain(hPort);                                                                                                 // Wait till all the data in the buffer is transmitted
392
393        if (b_rts){                                                                                                             // control the RTS signal if needed
394                ioctl(hPort, TIOCMBIC, &msc);                                                           // Clear Ready To Send signal
395        }
396
397        if (b_clear_echo)
398        {                                                                                               // Read echo from line and detect collisions if neened
399                unsigned char* echo_buffer = new unsigned char[numBytes];
400                int nBytesRead = 0;
401                int s = ECHO_WAIT_TIME_SEC;
402                int us = ECHO_WAIT_TIME_USEC;
403                while (nBytesRead < numBytesWritten)
404                {
405                        if (wait_for_input(&s , &us))
406                                nBytesRead = read(hPort, echo_buffer + nBytesRead, numBytes - nBytesRead);              // Read back ECHO of sended data
407                        else
408                        {
409                                #ifdef __DBG__
410                                printf("echo read back timeout\n");
411                                #endif
412                                delete[] echo_buffer;
413                                return ECHO_TIMEOUT_ERROR;
414                        }
415                }
416
417
418                #ifdef __DBG__
419                printf("echo  ");
420                for (int i=0;i<nBytesRead;i++)
421                        { printf("%02X ",buffer[i]); }
422                printf("(%d)\n",nBytesRead);
423                #endif
424
425                if (nBytesRead!=numBytesWritten)
426                {
427                        #ifdef __DBG__
428                        printf("Warning: nr. of characters echoed not correct\n");
429                        #endif
430                        delete[] echo_buffer;
431                        return READ_ERROR;
432                }
433
434                if (memcmp(buffer,echo_buffer, numBytes)!=0)
435                {
436                        #ifdef __DBG__
437                        printf("collition detect\n");
438                        #endif
439                        usleep(COLLISION_WAIT_TIME_USEC);                                                       //wait for a while
440                        tcflush(hPort, TCIFLUSH);                                                               //empty all data that was in read buffer but has not been read
441                        delete[] echo_buffer;
442                        return COLLISION_DETECT_ERROR;
443                }
444                delete[] echo_buffer;
445        }
446
447        return numBytesWritten;
448}
449
450void LxSerial::flush_buffer()
451{
452        tcflush(hPort, TCIOFLUSH);                                                                              // flush data buffer
453}
454
455LxSerial::~LxSerial()
456{
457        if (hPort != INVALID_DEVICE_HANDLE)
458                port_close();
459}
Note: See TracBrowser for help on using the repository browser.