net_serial.cpp
Go to the documentation of this file.
00001 /*
00002  *  RPLIDAR SDK
00003  *
00004  *  Copyright (c) 2009 - 2014 RoboPeak Team
00005  *  http://www.robopeak.com
00006  *  Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd.
00007  *  http://www.slamtec.com
00008  *
00009  */
00010 /*
00011  * Redistribution and use in source and binary forms, with or without
00012  * modification, are permitted provided that the following conditions are met:
00013  *
00014  * 1. Redistributions of source code must retain the above copyright notice,
00015  *    this list of conditions and the following disclaimer.
00016  *
00017  * 2. Redistributions in binary form must reproduce the above copyright notice,
00018  *    this list of conditions and the following disclaimer in the documentation
00019  *    and/or other materials provided with the distribution.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
00023  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
00024  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00025  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00026  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
00027  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
00028  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
00029  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
00030  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
00031  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00032  *
00033  */
00034 
00035 #include "arch/macOS/arch_macOS.h"
00036 #include "arch/macOS/net_serial.h"
00037 #include <termios.h>
00038 #include <sys/select.h>
00039 
00040 namespace rp{ namespace arch{ namespace net{
00041 
00042 raw_serial::raw_serial()
00043     : rp::hal::serial_rxtx()
00044     , _baudrate(0)
00045     , _flags(0)
00046     , serial_fd(-1)
00047 {
00048     _init();
00049 }
00050 
00051 raw_serial::~raw_serial()
00052 {
00053     close();
00054 
00055 }
00056 
00057 bool raw_serial::open()
00058 {
00059     return open(_portName, _baudrate, _flags);
00060 }
00061 
00062 bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags)
00063 {   
00064     strncpy(_portName, portname, sizeof(_portName));
00065     _baudrate = baudrate;
00066     _flags    = flags;
00067     return true;
00068 }
00069 
00070 bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags)
00071 {
00072     if (isOpened()) close();
00073     
00074     serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
00075 
00076     if (serial_fd == -1) return false;
00077 
00078     struct termios options, oldopt;
00079     tcgetattr(serial_fd, &oldopt);
00080         bzero(&options,sizeof(struct termios));
00081 
00082     _u32 termbaud = getTermBaudBitmap(baudrate);
00083 
00084     if (termbaud == (_u32)-1) {
00085         close();
00086         return false;
00087     }
00088     cfsetispeed(&options, termbaud);
00089     cfsetospeed(&options, termbaud);
00090         
00091         // enable rx and tx
00092         options.c_cflag |= (CLOCAL | CREAD);
00093 
00094 
00095     options.c_cflag &= ~PARENB; //no checkbit
00096         options.c_cflag &= ~CSTOPB; //1bit stop bit
00097 
00098         options.c_cflag &= ~CSIZE;
00099     options.c_cflag |= CS8; /* Select 8 data bits */
00100 
00101 #ifdef CNEW_RTSCTS
00102     options.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
00103 #endif
00104 
00105     options.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
00106 
00107     // raw input mode   
00108     options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
00109     // raw output mode   
00110     options.c_oflag &= ~OPOST;
00111     
00112     tcflush(serial_fd,TCIFLUSH); 
00113 /*
00114     if (fcntl(serial_fd, F_SETFL, FNDELAY))
00115     {
00116         close();
00117         return false;
00118     }
00119 */
00120     if (tcsetattr(serial_fd, TCSANOW, &options))
00121     {
00122         close();
00123         return false;
00124     }
00125     
00126     _is_serial_opened = true;
00127 
00128     //Clear the DTR bit to let the motor spin
00129     clearDTR();
00130     
00131     return true;
00132 }
00133 
00134 void raw_serial::close()
00135 {
00136     if (serial_fd != -1)
00137         ::close(serial_fd);
00138     serial_fd = -1;
00139     
00140     _is_serial_opened = false;
00141 }
00142 
00143 int raw_serial::senddata(const unsigned char * data, _word_size_t size)
00144 {
00145 // FIXME: non-block io should be used
00146     if (!isOpened()) return 0;
00147 
00148     if (data == NULL || size ==0) return 0;
00149     
00150     size_t tx_len = 0;
00151     required_tx_cnt = 0;
00152     do {
00153         int ans = ::write(serial_fd, data + tx_len, size-tx_len);
00154         
00155         if (ans == -1) return tx_len;
00156         
00157         tx_len += ans;
00158         required_tx_cnt = tx_len;
00159     }while (tx_len<size);
00160     
00161     
00162     return tx_len;
00163 }
00164 
00165 
00166 
00167 int raw_serial::recvdata(unsigned char * data, _word_size_t size)
00168 {
00169     if (!isOpened()) return 0;
00170     
00171     int ans = ::read(serial_fd, data, size);
00172     
00173     if (ans == -1) ans=0;
00174     required_rx_cnt = ans;
00175     return ans;
00176 }
00177 
00178 
00179 void raw_serial::flush( _u32 flags)
00180 {
00181     tcflush(serial_fd,TCIFLUSH); 
00182 }
00183 
00184 int raw_serial::waitforsent(_u32 timeout, _word_size_t * returned_size)
00185 {
00186     if (returned_size) *returned_size = required_tx_cnt;
00187     return 0;
00188 }
00189 
00190 int raw_serial::waitforrecv(_u32 timeout, _word_size_t * returned_size)
00191 {
00192     if (!isOpened() ) return -1;
00193    
00194     if (returned_size) *returned_size = required_rx_cnt;
00195     return 0;
00196 }
00197 
00198 int raw_serial::waitfordata(_word_size_t data_count, _u32 timeout, _word_size_t * returned_size)
00199 {
00200     _word_size_t length = 0;
00201     if (returned_size==NULL) returned_size=(_word_size_t *)&length;
00202     *returned_size = 0;
00203     
00204     int max_fd;
00205     fd_set input_set;
00206     struct timeval timeout_val;
00207 
00208     /* Initialize the input set */
00209     FD_ZERO(&input_set);
00210     FD_SET(serial_fd, &input_set);
00211     max_fd = serial_fd + 1;
00212 
00213     /* Initialize the timeout structure */
00214     timeout_val.tv_sec = timeout / 1000;
00215     timeout_val.tv_usec = (timeout % 1000) * 1000;
00216 
00217     if ( isOpened() )
00218     {
00219         if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
00220         if (*returned_size >= data_count)
00221         {
00222             return 0;
00223         }
00224     }
00225 
00226     while ( isOpened() )
00227     {
00228         /* Do the select */
00229         int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val);
00230 
00231         if (n < 0)
00232         {
00233             // select error
00234             return ANS_DEV_ERR;
00235         }
00236         else if (n == 0)
00237         {
00238             // time out
00239             return ANS_TIMEOUT;
00240         }
00241         else
00242         {
00243             // data avaliable
00244             assert (FD_ISSET(serial_fd, &input_set));
00245 
00246 
00247             if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
00248             if (*returned_size >= data_count)
00249             {
00250                 return 0;
00251             }
00252             else
00253             {
00254                 int remain_timeout = timeout_val.tv_sec*1000000 + timeout_val.tv_usec;
00255                 int expect_remain_time = (data_count - *returned_size)*1000000*8/_baudrate;
00256                 if (remain_timeout > expect_remain_time)
00257                     usleep(expect_remain_time);
00258             }
00259         }
00260         
00261     }
00262     return ANS_DEV_ERR;
00263 }
00264 
00265 size_t raw_serial::rxqueue_count()
00266 {
00267     if  ( !isOpened() ) return 0;
00268     size_t remaining;
00269     
00270     if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0;
00271     return remaining;
00272 }
00273 
00274 void raw_serial::setDTR()
00275 {
00276     if ( !isOpened() ) return;
00277 
00278     uint32_t dtr_bit = TIOCM_DTR;
00279     ioctl(serial_fd, TIOCMBIS, &dtr_bit);
00280 }
00281 
00282 void raw_serial::clearDTR()
00283 {
00284     if ( !isOpened() ) return;
00285 
00286     uint32_t dtr_bit = TIOCM_DTR;
00287     ioctl(serial_fd, TIOCMBIC, &dtr_bit);
00288 }
00289 
00290 void raw_serial::_init()
00291 {
00292     serial_fd = 0;
00293     _portName[0] = 0;
00294     required_tx_cnt = required_rx_cnt = 0;
00295 }
00296 
00297 
00298 
00299 _u32 raw_serial::getTermBaudBitmap(_u32 baud)
00300 {
00301 #define BAUD_CONV(_baud_) case _baud_:  return _baud_
00302     switch (baud)
00303     {
00304         BAUD_CONV(1200);
00305         BAUD_CONV(1800);
00306         BAUD_CONV(2400);
00307         BAUD_CONV(4800);
00308         BAUD_CONV(9600);
00309         BAUD_CONV(19200);
00310         BAUD_CONV(38400);
00311         BAUD_CONV(57600);
00312         BAUD_CONV(115200);
00313         BAUD_CONV(230400);
00314         BAUD_CONV(460800);
00315         BAUD_CONV(500000);
00316         BAUD_CONV(576000);
00317         BAUD_CONV(921600);
00318         BAUD_CONV(1000000);
00319         BAUD_CONV(1152000);
00320         BAUD_CONV(1500000);
00321         BAUD_CONV(2000000);
00322         BAUD_CONV(2500000);
00323         BAUD_CONV(3000000);
00324         BAUD_CONV(3500000);
00325         BAUD_CONV(4000000);
00326     }
00327     return -1;
00328 }
00329     
00330 }}} //end rp::arch::net
00331 
00332 
00333 
00334 //begin rp::hal
00335 namespace rp{ namespace hal{
00336     
00337     serial_rxtx * serial_rxtx::CreateRxTx()
00338     {
00339         return new rp::arch::net::raw_serial();
00340     }
00341     
00342     void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx)
00343     {
00344         delete rxtx;
00345     }
00346     
00347 }} //end rp::hal


rplidar_ros
Author(s):
autogenerated on Fri Dec 16 2016 03:59:16