void MAVConnSerial::async_write_end(error_code error, size_t bytes_transferred)
{
if (error) {
ROS_ERROR_STREAM_NAMED("mavconn", "serial" << channel << ":write: " << error.message());
close();
return;
}
lock_guard lock(mutex);
if (tx_q.empty()) {
tx_in_progress = false;
return;
}
MsgBuffer *buf = tx_q.front();
buf->pos += bytes_transferred;
if (buf->nbytes() == 0) {
tx_q.pop_front();
delete buf;
}
if (!tx_q.empty())
do_write(false);
else
tx_in_progress = false;
}
void MAVConnUDP::async_sendto_end(error_code error, size_t bytes_transferred)
{
if (error) {
logError(PFXd "sendto: %s", channel, error.message().c_str());
close();
return;
}
iostat_tx_add(bytes_transferred);
lock_guard lock(mutex);
if (tx_q.empty()) {
tx_in_progress = false;
return;
}
MsgBuffer *buf = tx_q.front();
buf->pos += bytes_transferred;
if (buf->nbytes() == 0) {
tx_q.pop_front();
delete buf;
}
if (!tx_q.empty())
do_sendto(false);
else
tx_in_progress = false;
}
void MAVConnSerial::async_write_end(error_code error, size_t bytes_transferred)
{
if (error) {
logError("serial%d:write: %s", channel, error.message().c_str());
close();
return;
}
lock_guard lock(mutex);
if (tx_q.empty()) {
tx_in_progress = false;
return;
}
MsgBuffer *buf = tx_q.front();
buf->pos += bytes_transferred;
if (buf->nbytes() == 0) {
tx_q.pop_front();
delete buf;
}
if (!tx_q.empty())
do_write(false);
else
tx_in_progress = false;
}
void MAVConnSerial::do_write(bool check_tx_state)
{
if (check_tx_state && tx_in_progress)
return;
lock_guard lock(mutex);
if (tx_q.empty())
return;
tx_in_progress = true;
MsgBuffer *buf = tx_q.front();
serial_dev.async_write_some(
buffer(buf->dpos(), buf->nbytes()),
boost::bind(&MAVConnSerial::async_write_end,
this,
boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred));
}
void MAVConnUDP::do_sendto(bool check_tx_state)
{
if (check_tx_state && tx_in_progress)
return;
lock_guard lock(mutex);
if (tx_q.empty())
return;
tx_in_progress = true;
MsgBuffer *buf = tx_q.front();
socket.async_send_to(
buffer(buf->dpos(), buf->nbytes()),
remote_ep,
boost::bind(&MAVConnUDP::async_sendto_end,
this,
boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred));
}
