port_handler_mac.cpp
Go to the documentation of this file.
00001 /*******************************************************************************
00002 * Copyright 2017 ROBOTIS CO., LTD.
00003 *
00004 * Licensed under the Apache License, Version 2.0 (the "License");
00005 * you may not use this file except in compliance with the License.
00006 * You may obtain a copy of the License at
00007 *
00008 *     http://www.apache.org/licenses/LICENSE-2.0
00009 *
00010 * Unless required by applicable law or agreed to in writing, software
00011 * distributed under the License is distributed on an "AS IS" BASIS,
00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013 * See the License for the specific language governing permissions and
00014 * limitations under the License.
00015 *******************************************************************************/
00016 
00017 /* Author: Ryu Woon Jung (Leon) */
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                             // You should adjust the latency timer value.
00039                             // When you are going to use sync / bulk read, the latency timer should be loosen.
00040                             // the lower latency timer value, the faster communication speed.
00041 
00042                             // Note:
00043                             // You can either change its value by following:
00044                             // http://www.ftdichip.com/Support/Documents/TechnicalNotes/TN_105%20Adding%20Support%20for%20New%20FTDI%20Devices%20to%20Mac%20Driver.pdf
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 // TODO: baud number ??
00087 bool PortHandlerMac::setBaudRate(const int baudrate)
00088 {
00089   int baud = getCFlagBaud(baudrate);
00090 
00091   closePort();
00092 
00093   if(baud <= 0)   // custom baudrate
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)); // clear struct for new port settings
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   // clean the buffer and activate the settings for the port
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     // Mac OS doesn't support over B230400
00231     // case 460800:
00232     //   return B460800;
00233     // case 500000:
00234     //   return B500000;
00235     // case 576000:
00236     //   return B576000;
00237     // case 921600:
00238     //   return B921600;
00239     // case 1000000:
00240     //   return B1000000;
00241     // case 1152000:
00242     //   return B1152000;
00243     // case 1500000:
00244     //   return B1500000;
00245     // case 2000000:
00246     //   return B2000000;
00247     // case 2500000:
00248     //   return B2500000;
00249     // case 3000000:
00250     //   return B3000000;
00251     // case 3500000:
00252     //   return B3500000;
00253     // case 4000000:
00254     //   return B4000000;
00255     default:
00256       return -1;
00257   }
00258 }
00259 
00260 #endif


ros
Author(s): Pyo , Zerom , Leon
autogenerated on Sat Jun 8 2019 18:32:11