00001
00009
00010
00011
00012
00013 #include <ecl/config/ecl.hpp>
00014 #ifdef ECL_IS_POSIX
00015
00016
00017
00018
00019
00020 #include <fcntl.h>
00021 #include <sys/ioctl.h>
00022 #include <termios.h>
00023 #include <cstdlib>
00024 #include <unistd.h>
00025 #include <ecl/exceptions/standard_exception.hpp>
00026 #include <ecl/exceptions/macros.hpp>
00027 #include "../../include/ecl/devices/serial_pos.hpp"
00028 #include "../../include/ecl/devices/detail/error_handler.hpp"
00029
00030
00031
00032
00033
00034 #include <ecl/config.hpp>
00035
00036
00037
00038
00039 #if defined(ECL_IS_APPLE)
00040 #ifndef B921600
00041 #define B921600 921600
00042 #endif
00043 #endif
00044
00045
00046
00047
00048
00049 namespace ecl
00050 {
00051
00052
00053
00054
00055
00056 using std::string;
00057 using ecl::InvalidInputError;
00058 using ecl::OpenError;
00059 using ecl::StandardException;
00060
00061
00062
00063
00064
00065 Serial::Serial(const std::string& port_name, const BaudRate &baud_rate, const DataBits &data_bits,
00066 const StopBits &stop_bits, const Parity &parity) ecl_throw_decl(StandardException) :
00067 port(port_name), read_timeout_ms(5000), error_handler(NoError)
00068 {
00069 ecl_try
00070 {
00071 open(port_name, baud_rate, data_bits, stop_bits, parity);
00072 } ecl_catch( const StandardException &e ) {
00073 ecl_throw(StandardException(LOC,e));
00074 }
00075 }
00076
00077 Serial::~Serial()
00078 {
00079 close();
00080 }
00081
00082
00083
00084
00085
00086 bool Serial::open(const std::string& port_name, const BaudRate &baud_rate, const DataBits &data_bits,
00087 const StopBits &stop_bits, const Parity &parity) ecl_throw_decl(StandardException)
00088 {
00089
00090
00091
00092
00093 if (stop_bits == StopBits_15)
00094 {
00095 ecl_throw(
00096 StandardException(LOC,ConfigurationError,"Standard serial device does not accept StopBits_15 as valid (used in ftdi)."));
00097 error_handler = InvalidArgError;
00098 is_open = false;
00099 return false;
00100 }
00101
00102 if (open())
00103 {
00104 close();
00105 }
00106 port = port_name;
00107
00108
00109
00110 file_descriptor = ::open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
00111 if (file_descriptor == -1)
00112 {
00113 ecl_throw(devices::open_exception(LOC,port_name));
00114 error_handler = devices::open_error();
00115 is_open = false;
00116 return false;
00117 }
00118
00119 static const int baud_rate_flags[] = {B110, B300, B600, B1200, B2400, B4800, B9600, B19200, B38400, B57600, B115200,
00120 B921600};
00121 static const int data_bits_flags[] = {CS5, CS6, CS7, CS8};
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133 fcntl(file_descriptor, F_SETFL, 0);
00134
00135
00136 struct flock file_lock;
00137 file_lock.l_type = F_WRLCK;
00138 file_lock.l_whence = SEEK_SET;
00139 file_lock.l_start = 0;
00140 file_lock.l_len = 0;
00141 file_lock.l_pid = getpid();
00142 if (fcntl(file_descriptor, F_SETLK, &file_lock) != 0)
00143 {
00144 ecl_throw_decl(
00145 StandardException(LOC,OpenError,std::string("Device is already locked. Try 'lsof | grep ") + port + std::string("' to find other processes that currently have the port open (if the device is a symbolic link you may need to replace the device name with the device that it is pointing to).")));
00146 error_handler = IsLockedError;
00147 is_open = false;
00148 return false;
00149 }
00150
00151
00152
00153
00154 options.c_cflag = 0;
00155 options.c_iflag = 0;
00156 options.c_lflag = 0;
00157 options.c_oflag = 0;
00158
00159
00160
00161
00162 cfsetispeed(&options, baud_rate_flags[baud_rate]);
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172 options.c_cflag |= CLOCAL;
00173 options.c_cflag |= CREAD;
00174
00175
00176
00177
00178 #if defined(CRTSCTS)
00179 options.c_cflag &= ~CRTSCTS;
00180 #elif defined (CNEW_RTSCTS)
00181 options.c_cflag &= ~CNEW_RTSCTS;
00182 #endif
00183
00184
00185
00186
00187 if (stop_bits == StopBits_1)
00188 {
00189 options.c_cflag &= ~CSTOPB;
00190 }
00191 else
00192 {
00193 options.c_cflag |= CSTOPB;
00194 }
00195
00196
00197
00198
00199 options.c_cflag &= ~CSIZE;
00200 options.c_cflag |= data_bits_flags[data_bits];
00201
00202
00203
00204
00205 options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
00206
00207
00208
00209
00210
00211 options.c_iflag &= ~(IXON | IXOFF | IXANY);
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221 if (parity == NoParity)
00222 {
00223 options.c_cflag &= ~PARENB;
00224 }
00225 else if (parity == EvenParity)
00226 {
00227 options.c_iflag |= (INPCK | ISTRIP);
00228 options.c_cflag |= PARENB;
00229 options.c_cflag &= ~PARODD;
00230 }
00231 else
00232 {
00233 options.c_iflag |= (INPCK | ISTRIP);
00234 options.c_cflag |= PARENB;
00235 options.c_cflag |= PARODD;
00236 }
00237
00238
00239
00240
00241
00242
00243
00244
00245 tcsetattr(file_descriptor, TCSAFLUSH, &options);
00246
00247
00248
00249
00250 if ( read_timeout_ms == NonBlocking ) {
00251 unblock();
00252 } else {
00253 block(read_timeout_ms);
00254 }
00255 clear();
00256
00257 is_open = true;
00258 error_handler = NoError;
00259 return true;
00260 }
00261
00262 void Serial::close()
00263 {
00264 if (is_open)
00265 {
00266
00267 ::close(file_descriptor);
00268 is_open = false;
00269 }
00270 }
00271
00272 bool Serial::open()
00273 {
00274 if ( is_open ) {
00275 if ( access(port.c_str(), F_OK ) == -1 ) {
00276 close();
00277 }
00278 }
00279 return is_open;
00280 }
00281
00282
00283
00284
00285
00286 void Serial::block(const unsigned long &timeout)
00287 {
00288 if (timeout < 100)
00289 {
00290 if (timeout < 5)
00291 {
00292 fake_snooze.period(ecl::Duration(0.001));
00293 fake_loop_count = timeout;
00294 }
00295 else if (timeout < 20)
00296 {
00297 fake_snooze.period(ecl::Duration(0.002));
00298 div_t d = div(timeout, 2);
00299 if (d.rem == 0)
00300 {
00301 fake_loop_count = d.quot;
00302 }
00303 else
00304 {
00305 fake_loop_count = d.quot + 1;
00306 }
00307 }
00308 else
00309 {
00310 fake_snooze.period(ecl::Duration(0.005));
00311 div_t d = div(timeout, 5);
00312 if (d.rem == 0)
00313 {
00314 fake_loop_count = d.quot;
00315 }
00316 else
00317 {
00318 fake_loop_count = d.quot + 1;
00319 }
00320 }
00321 this->unblock();
00322 }
00323 else
00324 {
00325 options.c_cc[VMIN] = 0;
00326
00327 if (timeout < 100)
00328 {
00329 options.c_cc[VTIME] = static_cast<unsigned char>(1);
00330 }
00331 else
00332 {
00333 options.c_cc[VTIME] = static_cast<unsigned char>(timeout / 100);
00334 }
00335 tcsetattr(file_descriptor, TCSAFLUSH, &options);
00336 }
00337 read_timeout_ms = timeout;
00338 }
00339
00340 void Serial::unblock()
00341 {
00342 options.c_cc[VMIN] = 0;
00343 options.c_cc[VTIME] = 0;
00344 tcsetattr(file_descriptor, TCSAFLUSH, &options);
00345 read_timeout_ms = NonBlocking;
00346 }
00347
00348
00349
00350
00351 long Serial::remaining()
00352 {
00353 long bytes = 0;
00354 ioctl(file_descriptor, FIONREAD, &bytes);
00355 return bytes;
00356 }
00357
00358 }
00359 #endif