19 #if defined(__linux__) 28 #include <sys/ioctl.h> 29 #include <linux/serial.h> 33 #define LATENCY_TIMER 16 // msec (USB latency timer) 70 #define TCGETS2 _IOR('T', 0x2A, struct termios2) 73 #define TCSETS2 _IOW('T', 0x2B, struct termios2) 76 #define BOTHER 0010000 81 PortHandlerLinux::PortHandlerLinux(
const char *port_name)
83 baudrate_(DEFAULT_BAUDRATE_),
84 packet_start_time_(0.0),
122 int baud = getCFlagBaud(baudrate);
130 return setCustomBaudrate(baudrate);
147 ioctl(
socket_fd_, FIONREAD, &bytes_available);
148 return bytes_available;
186 clock_gettime(CLOCK_REALTIME, &tv);
187 return ((
double)tv.tv_sec * 1000.0 + (
double)tv.tv_nsec * 0.001 * 0.001);
203 struct termios newtio;
208 printf(
"[PortHandlerLinux::SetupPort] Error opening serial port!\n");
212 bzero(&newtio,
sizeof(newtio));
214 newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD;
215 newtio.c_iflag = IGNPAR;
218 newtio.c_cc[VTIME] = 0;
219 newtio.c_cc[VMIN] = 0;
222 tcflush(socket_fd_, TCIFLUSH);
223 tcsetattr(socket_fd_, TCSANOW, &newtio);
231 struct termios2 options;
233 if (ioctl(socket_fd_, TCGETS2, &options) != 01)
235 options.c_cflag &= ~CBAUD;
236 options.c_cflag |= BOTHER;
237 options.c_ispeed = speed;
238 options.c_ospeed = speed;
240 if (ioctl(socket_fd_, TCSETS2, &options) != -1)
245 struct serial_struct ss;
246 if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0)
248 printf(
"[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n");
252 ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST;
253 ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed;
254 int closest_speed = ss.baud_base / ss.custom_divisor;
256 if(closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100)
258 printf(
"[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, closest is %d \n", speed, closest_speed);
262 if(ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0)
264 printf(
"[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n");
int writePort(uint8_t *packet, int length)
The function that writes bytes on the port buffer The function writes bytes on the port buffer...
bool setupPort(const int cflag_baud)
bool isPacketTimeout()
The function that checks whether packet timeout is occurred The function checks whether current time...
char * getPortName()
The function that returns port name set into the port handler The function returns current port name...
bool setCustomBaudrate(int speed)
void setPacketTimeout(uint16_t packet_length)
The function that sets and starts stopwatch for watching packet timeout The function sets the stopwa...
bool setBaudRate(const int baudrate)
The function that sets baudrate into the port handler The function sets baudrate into the port handl...
double getTimeSinceStart()
void closePort()
The function that closes the port The function closes the port.
void closePort()
The function that closes the port The function closes the port.
bool setBaudRate(const int baudrate)
The function that sets baudrate into the port handler The function sets baudrate into the port handl...
int getBytesAvailable()
The function that checks how much bytes are able to be read from the port buffer The function checks...
int getCFlagBaud(const int baudrate)
int readPort(uint8_t *packet, int length)
The function that reads bytes from the port buffer The function gets bytes from the port buffer...
void setPortName(const char *port_name)
The function that sets port name into the port handler The function sets port name into the port han...
double packet_start_time_
void setPortName(const char *port_name)
The function that sets port name into the port handler The function sets port name into the port han...
int getBaudRate()
The function that returns current baudrate set into the port handler The function returns current ba...
bool is_using_
shows whether the port is in use
double getTimeSinceStart()
void clearPort()
The function that clears the port The function clears the port.
bool openPort()
The function that opens the port The function calls PortHandlerLinux::setBaudRate() to open the port...
bool setupPort(const int cflag_baud)