Skip to content

Commit

Permalink
Increase priority of serial IO so that it does not get starved by the…
Browse files Browse the repository at this point in the history
… unreal engine.
Chris Lovett committed Feb 28, 2017
1 parent 3ec4c5a commit 70da873
Showing 6 changed files with 71 additions and 38 deletions.
12 changes: 6 additions & 6 deletions MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
Original file line number Diff line number Diff line change
@@ -44,17 +44,17 @@ MavLinkConnectionImpl::~MavLinkConnectionImpl()
close();
}

std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::createConnection(const std::string& nodeName, Port* port)
std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::createConnection(const std::string& nodeName, std::shared_ptr<Port> port)
{
// std::shared_ptr<MavLinkCom> owner, const std::string& nodeName
std::shared_ptr<MavLinkConnection> con = std::make_shared<MavLinkConnection>();
con->startListening(nodeName, std::shared_ptr<Port>(port));
con->startListening(nodeName, port);
return con;
}

std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectLocalUdp(const std::string& nodeName, std::string localAddr, int localPort)
{
UdpClientPort* socket = new UdpClientPort();
std::shared_ptr<UdpClientPort> socket = std::make_shared<UdpClientPort>();

socket->connect(localAddr, localPort, "", 0);

@@ -63,7 +63,7 @@ std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectLocalUdp(const

std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectRemoteUdp(const std::string& nodeName, std::string localAddr, std::string remoteAddr, int remotePort)
{
UdpClientPort* socket = new UdpClientPort();
std::shared_ptr<UdpClientPort> socket = std::make_shared<UdpClientPort>();

socket->connect(localAddr, 0, remoteAddr, remotePort);

@@ -72,7 +72,7 @@ std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectRemoteUdp(cons

std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectTcp(const std::string& nodeName, std::string localAddr, const std::string& remoteIpAddr, int remotePort)
{
TcpClientPort* socket = new TcpClientPort();
std::shared_ptr<TcpClientPort> socket = std::make_shared<TcpClientPort>();

socket->connect(localAddr, 0, remoteIpAddr, remotePort);

@@ -81,7 +81,7 @@ std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectTcp(const std:

std::shared_ptr<MavLinkConnection> MavLinkConnectionImpl::connectSerial(const std::string& nodeName, std::string name, int baudRate, const std::string initString)
{
SerialPort * serial = new SerialPort();
std::shared_ptr<SerialPort> serial = std::make_shared<SerialPort>();

int hr = serial->connect(name.c_str(), baudRate);
if (hr < 0)
2 changes: 1 addition & 1 deletion MavLinkCom/src/impl/MavLinkConnectionImpl.hpp
Original file line number Diff line number Diff line change
@@ -47,7 +47,7 @@ namespace mavlinkcom_impl {
void getTelemetry(MavLinkTelemetry& result);

private:
static std::shared_ptr<MavLinkConnection> createConnection(const std::string& nodeName, Port* port);
static std::shared_ptr<MavLinkConnection> createConnection(const std::string& nodeName, std::shared_ptr<Port> port);
void joinLeftSubscriber(std::shared_ptr<MavLinkConnection> remote, std::shared_ptr<MavLinkConnection>con, const MavLinkMessage& msg);
void joinRightSubscriber(std::shared_ptr<MavLinkConnection>con, const MavLinkMessage& msg);
void publishPackets();
39 changes: 26 additions & 13 deletions MavLinkCom/src/serial_com/SerialPort.cpp
Original file line number Diff line number Diff line change
@@ -58,14 +58,23 @@ void SerialPort::readPackets()
{
CurrentThread::setMaximumPriority();

port.async_read_some(
boost::asio::buffer(read_buf_raw, SERIAL_PORT_READ_BUF_SIZE),
boost::bind(
&SerialPort::on_receive,
this, boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred));
while (!closed_) {
boost::system::error_code ec;
size_t bytes = port.read_some(
boost::asio::buffer(read_buf_raw, SERIAL_PORT_READ_BUF_SIZE), ec);
on_receive(ec, bytes);
}

io_service.run();
// async mode is not used because boost will not run that async thread at maximum priority
// which is what we need in the Unreal environment.
//port.async_read_some(
// boost::asio::buffer(read_buf_raw, SERIAL_PORT_READ_BUF_SIZE),
// boost::bind(
// &SerialPort::on_receive,
// this, boost::asio::placeholders::error,
// boost::asio::placeholders::bytes_transferred));
//
//io_service.run();
}


@@ -164,12 +173,16 @@ SerialPort::read(uint8_t* buffer, int bytesToRead)
void
SerialPort::close()
{
port.close();
available.post();
if (read_thread.joinable())
{
read_thread.join();
if (!closed_) {
closed_ = true;
port.close();
available.post();
io_service.stop();
if (read_thread.joinable())
{
read_thread.join();
}
}
closed_ = true;

}

26 changes: 19 additions & 7 deletions MavLinkCom/src/serial_com/TcpClientPort.cpp
Original file line number Diff line number Diff line change
@@ -96,14 +96,26 @@ void TcpClientPort::connect(const std::string& localHost, int localPort, const s
void TcpClientPort::readPackets()
{
CurrentThread::setMaximumPriority();
socket_.async_receive(
boost::asio::buffer(read_buf_raw_, TCP_MAXBUF_SIZE),
boost::bind(
&TcpClientPort::on_receive,
this, boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred));

io_service_.run();
while (!closed_) {
boost::system::error_code ec;
size_t bytes = socket_.receive(
boost::asio::buffer(read_buf_raw_, TCP_MAXBUF_SIZE), 0, ec);
on_receive(ec, bytes);
}


// async mode is not used because boost will not run that async thread at maximum priority
// which is what we need in the Unreal environment.

//socket_.async_receive(
// boost::asio::buffer(read_buf_raw_, TCP_MAXBUF_SIZE),
// boost::bind(
// &TcpClientPort::on_receive,
// this, boost::asio::placeholders::error,
// boost::asio::placeholders::bytes_transferred));

//io_service_.run();
}

int TcpClientPort::write(const uint8_t* ptr, int count)
22 changes: 15 additions & 7 deletions MavLinkCom/src/serial_com/UdpClientPort.cpp
Original file line number Diff line number Diff line change
@@ -76,14 +76,22 @@ void UdpClientPort::connect(const std::string& localHost, int localPort, const s
void UdpClientPort::readPackets()
{
CurrentThread::setMaximumPriority();
socket.async_receive_from(
boost::asio::buffer(read_buf_raw, UDP_MAXBUF_SIZE), remote_endpoint,
boost::bind(
&UdpClientPort::on_receive,
this, boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred));
while (!closed) {
boost::system::error_code ec;
size_t bytes = socket.receive_from(
boost::asio::buffer(read_buf_raw, UDP_MAXBUF_SIZE), remote_endpoint, 0, ec);
on_receive(ec, bytes);
}

io_service.run();
// async mode is not used because boost will not run that async thread at maximum priority
// which is what we need in the Unreal environment.
//socket.async_receive_from(
// boost::asio::buffer(read_buf_raw, UDP_MAXBUF_SIZE), remote_endpoint,
// boost::bind(
// &UdpClientPort::on_receive,
// this, boost::asio::placeholders::error,
// boost::asio::placeholders::bytes_transferred));
//io_service.run();
}

int UdpClientPort::write(const uint8_t* ptr, int count)
8 changes: 4 additions & 4 deletions cmake/build.sh
Original file line number Diff line number Diff line change
@@ -32,8 +32,8 @@ pushd ..
mkdir -p AirLib/lib/x64/Debug
mkdir -p AirLib/deps/rpclib
mkdir -p AirLib/deps/MavLinkCom
cp -p cmake/output/lib/* AirLib/lib/x64/Debug
cp -rp external/rpclib/include AirLib/deps/rpclib
cp -rp MavLinkCom/include AirLib/deps/MavLinkCom
cp -rp AirLib Unreal/Plugins/AirSim/Source
rsync -a --delete cmake/output/lib/ AirLib/lib/x64/Debug
rsync -a --delete external/rpclib/include AirLib/deps/rpclib
rsync -a --delete MavLinkCom/include AirLib/deps/MavLinkCom
rsync -a --delete AirLib Unreal/Plugins/AirSim/Source
popd

0 comments on commit 70da873

Please sign in to comment.