Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #if defined(__APPLE__)
00020
00021 #include <stdio.h>
00022 #include <fcntl.h>
00023 #include <string.h>
00024 #include <unistd.h>
00025 #include <termios.h>
00026 #include <time.h>
00027 #include <sys/time.h>
00028 #include <sys/ioctl.h>
00029
00030 #ifdef __MACH__
00031 #include <mach/clock.h>
00032 #include <mach/mach.h>
00033 #endif
00034
00035 #include "port_handler_mac.h"
00036
00037 #define LATENCY_TIMER 16 // msec (USB latency timer)
00038
00039
00040
00041
00042
00043
00044
00045
00046 using namespace dynamixel;
00047
00048 PortHandlerMac::PortHandlerMac(const char *port_name)
00049 : socket_fd_(-1),
00050 baudrate_(DEFAULT_BAUDRATE_),
00051 packet_start_time_(0.0),
00052 packet_timeout_(0.0),
00053 tx_time_per_byte(0.0)
00054 {
00055 is_using_ = false;
00056 setPortName(port_name);
00057 }
00058
00059 bool PortHandlerMac::openPort()
00060 {
00061 return setBaudRate(baudrate_);
00062 }
00063
00064 void PortHandlerMac::closePort()
00065 {
00066 if(socket_fd_ != -1)
00067 close(socket_fd_);
00068 socket_fd_ = -1;
00069 }
00070
00071 void PortHandlerMac::clearPort()
00072 {
00073 tcflush(socket_fd_, TCIFLUSH);
00074 }
00075
00076 void PortHandlerMac::setPortName(const char *port_name)
00077 {
00078 strcpy(port_name_, port_name);
00079 }
00080
00081 char *PortHandlerMac::getPortName()
00082 {
00083 return port_name_;
00084 }
00085
00086
00087 bool PortHandlerMac::setBaudRate(const int baudrate)
00088 {
00089 int baud = getCFlagBaud(baudrate);
00090
00091 closePort();
00092
00093 if(baud <= 0)
00094 {
00095 setupPort(B38400);
00096 baudrate_ = baudrate;
00097 return setCustomBaudrate(baudrate);
00098 }
00099 else
00100 {
00101 baudrate_ = baudrate;
00102 return setupPort(baud);
00103 }
00104 }
00105
00106 int PortHandlerMac::getBaudRate()
00107 {
00108 return baudrate_;
00109 }
00110
00111 int PortHandlerMac::getBytesAvailable()
00112 {
00113 int bytes_available;
00114 ioctl(socket_fd_, FIONREAD, &bytes_available);
00115 return bytes_available;
00116 }
00117
00118 int PortHandlerMac::readPort(uint8_t *packet, int length)
00119 {
00120 return read(socket_fd_, packet, length);
00121 }
00122
00123 int PortHandlerMac::writePort(uint8_t *packet, int length)
00124 {
00125 return write(socket_fd_, packet, length);
00126 }
00127
00128 void PortHandlerMac::setPacketTimeout(uint16_t packet_length)
00129 {
00130 packet_start_time_ = getCurrentTime();
00131 packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0;
00132 }
00133
00134 void PortHandlerMac::setPacketTimeout(double msec)
00135 {
00136 packet_start_time_ = getCurrentTime();
00137 packet_timeout_ = msec;
00138 }
00139
00140 bool PortHandlerMac::isPacketTimeout()
00141 {
00142 if(getTimeSinceStart() > packet_timeout_)
00143 {
00144 packet_timeout_ = 0;
00145 return true;
00146 }
00147 return false;
00148 }
00149
00150 double PortHandlerMac::getCurrentTime()
00151 {
00152 struct timespec tv;
00153 #ifdef __MACH__ // OS X does not have clock_gettime, so here uses clock_get_time
00154 clock_serv_t cclock;
00155 mach_timespec_t mts;
00156 host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
00157 clock_get_time(cclock, &mts);
00158 mach_port_deallocate(mach_task_self(), cclock);
00159 tv.tv_sec = mts.tv_sec;
00160 tv.tv_nsec = mts.tv_nsec;
00161 #else
00162 clock_gettime(CLOCK_REALTIME, &tv);
00163 #endif
00164 return ((double)tv.tv_sec * 1000.0 + (double)tv.tv_nsec * 0.001 * 0.001);
00165 }
00166
00167 double PortHandlerMac::getTimeSinceStart()
00168 {
00169 double time;
00170
00171 time = getCurrentTime() - packet_start_time_;
00172 if(time < 0.0)
00173 packet_start_time_ = getCurrentTime();
00174
00175 return time;
00176 }
00177
00178 bool PortHandlerMac::setupPort(int cflag_baud)
00179 {
00180 struct termios newtio;
00181
00182 socket_fd_ = open(port_name_, O_RDWR|O_NOCTTY|O_NONBLOCK);
00183 if(socket_fd_ < 0)
00184 {
00185 printf("[PortHandlerMac::SetupPort] Error opening serial port!\n");
00186 return false;
00187 }
00188
00189 bzero(&newtio, sizeof(newtio));
00190
00191 newtio.c_cflag = CS8 | CLOCAL | CREAD;
00192 newtio.c_iflag = IGNPAR;
00193 newtio.c_oflag = 0;
00194 newtio.c_lflag = 0;
00195 newtio.c_cc[VTIME] = 0;
00196 newtio.c_cc[VMIN] = 0;
00197 cfsetispeed(&newtio, cflag_baud);
00198 cfsetospeed(&newtio, cflag_baud);
00199
00200
00201 tcflush(socket_fd_, TCIFLUSH);
00202 tcsetattr(socket_fd_, TCSANOW, &newtio);
00203
00204 tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0;
00205 return true;
00206 }
00207
00208 bool PortHandlerMac::setCustomBaudrate(int speed)
00209 {
00210 printf("[PortHandlerMac::SetCustomBaudrate] Not supported on Mac!\n");
00211 return false;
00212 }
00213
00214 int PortHandlerMac::getCFlagBaud(int baudrate)
00215 {
00216 switch(baudrate)
00217 {
00218 case 9600:
00219 return B9600;
00220 case 19200:
00221 return B19200;
00222 case 38400:
00223 return B38400;
00224 case 57600:
00225 return B57600;
00226 case 115200:
00227 return B115200;
00228 case 230400:
00229 return B230400;
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255 default:
00256 return -1;
00257 }
00258 }
00259
00260 #endif