git commit -m "first commit for v2"
This commit is contained in:
309
Devices/Libraries/Ros/libserial/src/rs485.cpp
Executable file
309
Devices/Libraries/Ros/libserial/src/rs485.cpp
Executable file
@@ -0,0 +1,309 @@
|
||||
#include "libserial/rs485.h"
|
||||
|
||||
/*
|
||||
* Configure
|
||||
* @param devfile Path name of dev
|
||||
* @param baud Baudrate
|
||||
* @param parity Parity
|
||||
* @param data_bit
|
||||
* @param stop_bit
|
||||
*/
|
||||
int rs485::_rs485_connect(rs485_t &ctx)
|
||||
{
|
||||
struct termios tios;
|
||||
int flags;
|
||||
/* The O_NOCTTY flag tells UNIX that this program doesn't want
|
||||
to be the "controlling terminal" for that port. If you
|
||||
don't specify this then any input (such as keyboard abort
|
||||
signals and so forth) will affect your process
|
||||
Timeouts are ignored in canonical input mode or when the
|
||||
NDELAY option is set on the file via open or fcntl
|
||||
*/
|
||||
flags = O_RDWR | O_NOCTTY | O_NONBLOCK | O_NDELAY;
|
||||
ctx.s = open(ctx._port,flags);
|
||||
if (ctx.s < 0)
|
||||
{
|
||||
std::stringstream e;
|
||||
e << ctx._port <<" is failed: " << strerror(errno);
|
||||
error_msg = e.str();
|
||||
_connected = false;
|
||||
return -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
fcntl(ctx.s, F_SETFL, 0);
|
||||
}
|
||||
/* Save */
|
||||
if(tcgetattr(ctx.s, &ctx._old_tios) <0)
|
||||
{
|
||||
ROS_ERROR("Can't get terminal parameters");
|
||||
}
|
||||
|
||||
/*
|
||||
* Enable the receiver and set local mode...
|
||||
*/
|
||||
bzero(&tios, sizeof(tios));
|
||||
|
||||
/* C_ISPEED Input baud (new interface)
|
||||
* C_OSPEED Output baud (new interface)
|
||||
*/
|
||||
speed_t speed = setBaudRate(ctx._baud);
|
||||
/* Set the baud rate */
|
||||
if ((cfsetispeed(&tios, speed) < 0) ||
|
||||
(cfsetospeed(&tios, speed) < 0))
|
||||
{
|
||||
close(ctx.s);
|
||||
ctx.s = -1;
|
||||
_connected = false;
|
||||
return -1;
|
||||
}
|
||||
setDataBits(tios,ctx._data_bit);
|
||||
setStopBits(tios,ctx._stop_bit);
|
||||
setParity(tios, ctx._parity);
|
||||
|
||||
/* Read the man page of termios if you need more information.
|
||||
* This field isn't used on POSIX systems
|
||||
* tios.c_line = 0;
|
||||
*/
|
||||
|
||||
/* C_LFLAG Line options
|
||||
ISIG Enable SIGINTR, SIGSUSP, SIGDSUSP, and SIGQUIT signals
|
||||
ICANON Enable canonical input (else raw)
|
||||
XCASE Map uppercase \lowercase (obsolete)
|
||||
ECHO Enable echoing of input characters
|
||||
ECHOE Echo erase character as BS-SP-BS
|
||||
ECHOK Echo NL after kill character
|
||||
ECHONL Echo NL
|
||||
NOFLSH Disable flushing of input buffers after
|
||||
interrupt or quit characters
|
||||
IEXTEN Enable extended functions
|
||||
ECHOCTL Echo control characters as ^char and delete as ~?
|
||||
ECHOPRT Echo erased character as character erased
|
||||
ECHOKE BS-SP-BS entire line on line kill
|
||||
FLUSHO Output being flushed
|
||||
PENDIN Retype pending input at next read or input char
|
||||
TOSTOP Send SIGTTOU for background output
|
||||
Canonical input is line-oriented. Input characters are put
|
||||
into a buffer which can be edited interactively by the user
|
||||
until a CR (carriage return) or LF (line feed) character is
|
||||
received.
|
||||
Raw input is unprocessed. Input characters are passed
|
||||
through exactly as they are received, when they are
|
||||
received. Generally you'll deselect the ICANON, ECHO,
|
||||
ECHOE, and ISIG options when using raw input
|
||||
*/
|
||||
|
||||
/* Raw input */
|
||||
tios.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);;
|
||||
|
||||
/* Software flow control is disabled */
|
||||
tios.c_iflag &= ~(IXON | IXOFF | IXANY);
|
||||
|
||||
/* C_OFLAG Output options
|
||||
OPOST Postprocess output (not set = raw output)
|
||||
ONLCR Map NL to CR-NL
|
||||
ONCLR ant others needs OPOST to be enabled
|
||||
*/
|
||||
|
||||
/* Raw ouput */
|
||||
tios.c_oflag = 0;
|
||||
|
||||
/* C_CC Control characters
|
||||
VMIN Minimum number of characters to read
|
||||
VTIME Time to wait for data (tenths of seconds)
|
||||
UNIX serial interface drivers provide the ability to
|
||||
specify character and packet timeouts. Two elements of the
|
||||
c_cc array are used for timeouts: VMIN and VTIME. Timeouts
|
||||
are ignored in canonical input mode or when the NDELAY
|
||||
option is set on the file via open or fcntl.
|
||||
VMIN specifies the minimum number of characters to read. If
|
||||
it is set to 0, then the VTIME value specifies the time to
|
||||
wait for every character read. Note that this does not mean
|
||||
that a read call for N bytes will wait for N characters to
|
||||
come in. Rather, the timeout will apply to the first
|
||||
character and the read call will return the number of
|
||||
characters immediately available (up to the number you
|
||||
request).
|
||||
If VMIN is non-zero, VTIME specifies the time to wait for
|
||||
the first character read. If a character is read within the
|
||||
time given, any read will block (wait) until all VMIN
|
||||
characters are read. That is, once the first character is
|
||||
read, the serial interface driver expects to receive an
|
||||
entire packet of characters (VMIN bytes total). If no
|
||||
character is read within the time allowed, then the call to
|
||||
read returns 0. This method allows you to tell the serial
|
||||
driver you need exactly N bytes and any read call will
|
||||
return 0 or N bytes. However, the timeout only applies to
|
||||
the first character read, so if for some reason the driver
|
||||
misses one character inside the N byte packet then the read
|
||||
call could block forever waiting for additional input
|
||||
characters.
|
||||
VTIME specifies the amount of time to wait for incoming
|
||||
characters in tenths of seconds. If VTIME is set to 0 (the
|
||||
default), reads will block (wait) indefinitely unless the
|
||||
NDELAY option is set on the port with open or fcntl.
|
||||
*/
|
||||
/* Unused because we use open with the NDELAY option */
|
||||
tios.c_cc[VMIN] = 5; /* The character minximum to read*/
|
||||
tios.c_cc[VTIME] = 20; /* The wait time maximum to read*/
|
||||
/* clean port */
|
||||
tcflush(ctx.s, TCIFLUSH);
|
||||
/* activate the settings port */
|
||||
if (tcsetattr(ctx.s, TCSANOW, &tios) < 0) {
|
||||
ROS_ERROR("Can't get terminal parameters");
|
||||
close(ctx.s);
|
||||
ctx.s = -1;
|
||||
_connected = false;
|
||||
return -1;
|
||||
}
|
||||
ctx._old_tios = tios;
|
||||
_connected = true;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* Reconnect to device if rs485 modules was disconected.
|
||||
*/
|
||||
void rs485::reconnect(void)
|
||||
{
|
||||
_rs485_connect(ctx);
|
||||
}
|
||||
|
||||
/*
|
||||
* create new port
|
||||
* @param devfile Path name of dev
|
||||
* @param baud Baudrate
|
||||
* @param parity Parity
|
||||
* @param data_bit
|
||||
* @param stop_bit
|
||||
*/
|
||||
void rs485::new_port(const char* devfile,unsigned int baud, parity_t parity,
|
||||
data_bits_t data_bit,stop_bits_t stop_bit)
|
||||
{
|
||||
ctx._port = devfile;
|
||||
ctx._baud = baud;
|
||||
ctx._parity = parity;
|
||||
ctx._data_bit = data_bit;
|
||||
ctx._stop_bit = stop_bit;
|
||||
_connected = false;
|
||||
if(parity == PARITY_EVEN || parity == PARITY_ODD)
|
||||
size_pkg++;
|
||||
switch(stop_bit)
|
||||
{
|
||||
case STOPBIT_1:
|
||||
size_pkg++;
|
||||
break;
|
||||
case STOPBIT_2:
|
||||
size_pkg +=2;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
switch(data_bit)
|
||||
{
|
||||
case DATABIT_5:
|
||||
size_pkg += 5;
|
||||
break;
|
||||
case DATABIT_6:
|
||||
size_pkg += 6;
|
||||
break;
|
||||
case DATABIT_7:
|
||||
size_pkg += 7;
|
||||
break;
|
||||
case DATABIT_8:
|
||||
size_pkg += 8;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
size_pkg +=2; // bit start and stop
|
||||
|
||||
switch(baud)
|
||||
{
|
||||
case 19200:
|
||||
param_safety = 1.5;
|
||||
break;
|
||||
case 115200:
|
||||
param_safety = 3.5;
|
||||
break;
|
||||
default:
|
||||
param_safety = 1.5;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Close serial port
|
||||
*/
|
||||
void rs485::close_port()
|
||||
{
|
||||
close(this->ctx.s);
|
||||
}
|
||||
|
||||
/*
|
||||
* Data Sender
|
||||
* @param to_send Request to Be Sent to Server
|
||||
* @param length Length of the Request
|
||||
* @return Size of the request
|
||||
*/
|
||||
ssize_t rs485::sendMsgs(uint8_t *to_send, uint16_t length)
|
||||
{
|
||||
float time = param_safety*1000000/(ctx._baud / (size_pkg * length));
|
||||
usleep(time);
|
||||
memset((to_send + length),'\0', 1);
|
||||
ssize_t num = write(ctx.s, to_send, (size_t)length);
|
||||
usleep(time);
|
||||
return num;
|
||||
}
|
||||
|
||||
/*
|
||||
* Data Receiver
|
||||
* @param buffer Buffer to Store the Data` Retrieved
|
||||
* @return Size of Incoming Data
|
||||
*/
|
||||
ssize_t rs485::receiveMsgs(uint8_t *buffer) const
|
||||
{
|
||||
memset(buffer,'\0',sizeof(buffer));
|
||||
return read(ctx.s, (char *) buffer,MAX_MSG_LENGTH);
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief Constructor
|
||||
* @param[in] devfile Example /dev/tty*
|
||||
* @param[in] baud Number of transmitted bits per a second
|
||||
* @param[in] parity The parity check bit {Even, None , Old }
|
||||
* @param[in] data_bit Number of bits in a transmission frame
|
||||
* @param[in] stop_bit End bit
|
||||
*/
|
||||
rs485::rs485(const char* devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit)
|
||||
{
|
||||
new_port(devfile, baud, parity, data_bit, stop_bit);
|
||||
_rs485_connect(ctx);
|
||||
}
|
||||
|
||||
/*
|
||||
* Destructor.
|
||||
*/
|
||||
rs485::~rs485()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* Set read character miximum
|
||||
* @param[in] num The character minximum to receive.
|
||||
*/
|
||||
int rs485::setCharacterNumToRead(int num)
|
||||
{
|
||||
ctx._old_tios.c_cc[VMIN] = num;
|
||||
/* clean port */
|
||||
tcflush(ctx.s, TCIFLUSH);
|
||||
/* activate the settings port */
|
||||
if (tcsetattr(ctx.s, TCSANOW, &ctx._old_tios) < 0) {
|
||||
ROS_ERROR("Can't get terminal parameters");
|
||||
close(ctx.s);
|
||||
ctx.s = -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
181
Devices/Libraries/Ros/libserial/src/serial.cpp
Executable file
181
Devices/Libraries/Ros/libserial/src/serial.cpp
Executable file
@@ -0,0 +1,181 @@
|
||||
#include "libserial/serial.h"
|
||||
|
||||
/*
|
||||
* @brief Setting baudrate
|
||||
* @param[in] b baud speed input
|
||||
*/
|
||||
speed_t Serial::setBaudRate(unsigned int b) {
|
||||
speed_t speed;
|
||||
switch (b) {
|
||||
case 110:
|
||||
speed = B110;
|
||||
break;
|
||||
case 300:
|
||||
speed = B300;
|
||||
break;
|
||||
case 600:
|
||||
speed = B600;
|
||||
break;
|
||||
case 1200:
|
||||
speed = B1200;
|
||||
break;
|
||||
case 2400:
|
||||
speed = B2400;
|
||||
break;
|
||||
case 4800:
|
||||
speed = B4800;
|
||||
break;
|
||||
case 9600:
|
||||
speed = B9600;
|
||||
break;
|
||||
case 19200:
|
||||
speed = B19200;
|
||||
break;
|
||||
case 8400:
|
||||
speed = B38400;
|
||||
break;
|
||||
case 57600:
|
||||
speed = B57600;
|
||||
break;
|
||||
case 115200:
|
||||
speed = B115200;
|
||||
break;
|
||||
case 230400:
|
||||
speed = B230400;
|
||||
break;
|
||||
case 460800:
|
||||
speed = B460800;
|
||||
break;
|
||||
case 500000:
|
||||
speed = B500000;
|
||||
break;
|
||||
case 576000:
|
||||
speed = B576000;
|
||||
break;
|
||||
case 921600:
|
||||
speed = B921600;
|
||||
break;
|
||||
case 1000000:
|
||||
speed = B1000000;
|
||||
break;
|
||||
case 1152000:
|
||||
speed = B1152000;
|
||||
break;
|
||||
case 1500000:
|
||||
speed = B1500000;
|
||||
break;
|
||||
case 2500000:
|
||||
speed = B2500000;
|
||||
break;
|
||||
case 3000000:
|
||||
speed = B3000000;
|
||||
break;
|
||||
case 3500000:
|
||||
speed = B3500000;
|
||||
break;
|
||||
case 4000000:
|
||||
speed = B4000000;
|
||||
break;
|
||||
default:
|
||||
speed = B9600;
|
||||
break;
|
||||
}
|
||||
return speed;
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief Setting parity bits type
|
||||
* @param[in] b baud speed input
|
||||
*/
|
||||
void Serial::setParity(termios& tios, parity p) {
|
||||
/* PARENB Enable parity bit
|
||||
PARODD Use odd parity instead of even */
|
||||
switch (p)
|
||||
{
|
||||
/* None */
|
||||
case PARITY_NONE:
|
||||
tios.c_cflag &= ~PARENB;
|
||||
tios.c_iflag &= ~INPCK;
|
||||
break;
|
||||
/* Even */
|
||||
case PARITY_EVEN:
|
||||
tios.c_cflag |= PARENB;
|
||||
tios.c_cflag &= ~PARODD;
|
||||
tios.c_iflag |= INPCK;
|
||||
break;
|
||||
/* Odd */
|
||||
case PARITY_ODD:
|
||||
tios.c_cflag |= PARENB;
|
||||
tios.c_cflag |= PARODD;
|
||||
break;
|
||||
/* Default None */
|
||||
default:
|
||||
tios.c_cflag &= ~PARENB;
|
||||
tios.c_iflag &= ~INPCK;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief Setting stop bits type
|
||||
* @param[in] b baud speed input
|
||||
*/
|
||||
void Serial::setStopBits(termios& tios, stop_bits s) {
|
||||
/* Stop bit (1 or 2) */
|
||||
switch (s)
|
||||
{
|
||||
case STOPBIT_1:
|
||||
tios.c_cflag &= ~CSTOPB;
|
||||
break;
|
||||
|
||||
case STOPBIT_2:
|
||||
tios.c_cflag |= CSTOPB;
|
||||
break;
|
||||
|
||||
default:
|
||||
tios.c_cflag &= ~CSTOPB;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief Setting data bits type
|
||||
* @param[in] b baud speed input
|
||||
*/
|
||||
void Serial::setDataBits(termios& tios, data_bits d)
|
||||
{
|
||||
/* C_CFLAG Control options
|
||||
* CLOCAL Local line - do not change "owner" of port
|
||||
* CREAD Enable receiver
|
||||
*/
|
||||
tios.c_cflag |= (CREAD | CLOCAL);
|
||||
/* CSIZE, HUPCL, CRTSCTS (hardware flow control) */
|
||||
|
||||
/* Set data bits (5, 6, 7, 8 bits)
|
||||
* CSIZE Bit mask for data bits
|
||||
*/
|
||||
tios.c_cflag &= ~CSIZE;
|
||||
switch (d)
|
||||
{
|
||||
case DATABIT_8:
|
||||
tios.c_cflag |= CS8;
|
||||
break;
|
||||
|
||||
case DATABIT_7:
|
||||
tios.c_cflag |= CS7;
|
||||
break;
|
||||
|
||||
case DATABIT_6:
|
||||
tios.c_cflag |= CS6;
|
||||
break;
|
||||
|
||||
case DATABIT_5:
|
||||
tios.c_cflag |= CS5;
|
||||
break;
|
||||
|
||||
default:
|
||||
tios.c_cflag |= CS8;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user