156 lines
3.9 KiB
C++
156 lines
3.9 KiB
C++
#include "Serial.h"
|
|
#include "StreamBasedLauncher.h"
|
|
#include "Utilities.h"
|
|
#include "Stream.h"
|
|
#include "ReliableStream.h"
|
|
#include "ServerProtocol.h"
|
|
#include <termios.h>
|
|
#include <unistd.h>
|
|
#include <fcntl.h>
|
|
#include <arpa/inet.h>
|
|
#include <poll.h>
|
|
#include <chrono>
|
|
#include <iostream>
|
|
|
|
namespace po = boost::program_options;
|
|
using namespace std::literals::chrono_literals;
|
|
|
|
class SerialStream : public WaitableStream
|
|
{
|
|
static const long kReadBufferSize = 4096;
|
|
uint8_t readBuffer[kReadBufferSize];
|
|
public:
|
|
int fd;
|
|
int baud;
|
|
|
|
virtual void write(const void* p, size_t n) override;
|
|
|
|
virtual void wait() override;
|
|
|
|
SerialStream(po::variables_map &options);
|
|
~SerialStream();
|
|
};
|
|
|
|
class SerialLauncher : public StreamBasedLauncher
|
|
{
|
|
SerialStream stream;
|
|
ReliableStream rStream;
|
|
public:
|
|
SerialLauncher(po::variables_map& options);
|
|
};
|
|
|
|
|
|
SerialStream::SerialStream(po::variables_map &options)
|
|
{
|
|
std::string port = options["serial-port"].as<std::string>();
|
|
baud = options["serial-baud"].as<int>();
|
|
fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY );
|
|
if(fd < 0)
|
|
throw std::runtime_error("Cannot open serial port.\n");
|
|
|
|
struct termios tios;
|
|
tcgetattr(fd,&tios);
|
|
|
|
tios.c_cflag = CRTSCTS | CS8 | CLOCAL | CREAD;
|
|
|
|
switch(baud)
|
|
{
|
|
case 9600: tios.c_cflag |= B9600; break;
|
|
case 19200: tios.c_cflag |= B19200; break;
|
|
case 38400: tios.c_cflag |= B38400; break;
|
|
case 57600: tios.c_cflag |= B57600; break;
|
|
case 115200: tios.c_cflag |= B115200; break;
|
|
case 230400: tios.c_cflag |= B230400; break;
|
|
default:
|
|
throw std::runtime_error("Unsupported baud rate.\n");
|
|
}
|
|
|
|
tios.c_iflag = 0;//IGNPAR | ICRNL;
|
|
tios.c_lflag = 0;
|
|
tios.c_oflag = 0;
|
|
tios.c_cc[VTIME] = 0; /* inter-character timer unused */
|
|
tios.c_cc[VMIN] = 1; /* blocking read until 1 chars received */
|
|
tcsetattr(fd,TCSANOW,&tios);
|
|
usleep(500000);
|
|
}
|
|
SerialStream::~SerialStream()
|
|
{
|
|
tcdrain(fd);
|
|
usleep(500000);
|
|
close(fd);
|
|
}
|
|
|
|
void SerialStream::write(const void* p, size_t n)
|
|
{
|
|
while(n)
|
|
{
|
|
struct pollfd pfd;
|
|
pfd.fd = fd;
|
|
pfd.events = POLLOUT;
|
|
pfd.revents = 0;
|
|
poll(&pfd, 1, 1000);
|
|
if(pfd.revents & POLLOUT)
|
|
{
|
|
ssize_t written = ::write(fd, p, n);
|
|
if(written > 0)
|
|
{
|
|
p = (const void*) ( (const char*)p + written );
|
|
n -= written;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void SerialStream::wait()
|
|
{
|
|
struct pollfd pfd;
|
|
pfd.fd = fd;
|
|
pfd.events = POLLIN;
|
|
pfd.revents = 0;
|
|
poll(&pfd, 1, 1000);
|
|
if(pfd.revents & POLLIN)
|
|
{
|
|
ssize_t n = ::read(fd, readBuffer, kReadBufferSize);
|
|
if(n > 0)
|
|
{
|
|
notifyReceive(readBuffer, n);
|
|
}
|
|
}
|
|
}
|
|
|
|
SerialLauncher::SerialLauncher(po::variables_map &options)
|
|
: StreamBasedLauncher(options), stream(options), rStream(&stream)
|
|
{
|
|
SetupStream(&stream, &rStream);
|
|
|
|
do
|
|
{
|
|
rStream.reset(1);
|
|
std::cerr << "Connecting... (" << stream.baud << " baud)" << std::endl;
|
|
using clock = std::chrono::steady_clock;
|
|
auto startTime = clock::now();
|
|
while(!rStream.resetResponseArrived() && clock::now() - startTime < 5s)
|
|
stream.wait();
|
|
} while(!rStream.resetResponseArrived());
|
|
|
|
std::cerr << "Connected." << std::endl;
|
|
}
|
|
|
|
void Serial::GetOptions(options_description &desc)
|
|
{
|
|
desc.add_options()
|
|
("serial-port", po::value<std::string>()->default_value("/dev/ttyUSB0"), "serial port to use")
|
|
("serial-baud", po::value<int>()->default_value(19200), "serial port speed")
|
|
;
|
|
}
|
|
|
|
bool Serial::CheckOptions(variables_map &options)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
std::unique_ptr<Launcher> Serial::MakeLauncher(variables_map &options)
|
|
{
|
|
return std::unique_ptr<Launcher>(new SerialLauncher(options));
|
|
}
|