diff --git a/boards/udp/udp_board.cpp b/boards/udp/udp_board.cpp new file mode 100644 index 00000000..cba9c48e --- /dev/null +++ b/boards/udp/udp_board.cpp @@ -0,0 +1,190 @@ +/* + * Copyright (c) 2017 Daniel Koch, BYU MAGICC Lab. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * \file udp_board.cpp + * \author Daniel Koch + */ + +#include +#include "udp_board.hpp" + +using boost::asio::ip::udp; + +namespace rosflight_firmware +{ +UDPBoard::UDPBoard(std::string bind_host, uint16_t bind_port, std::string remote_host, + uint16_t remote_port) + : bind_host_(std::move(bind_host)), bind_port_(bind_port), remote_host_(std::move(remote_host)), + remote_port_(remote_port), io_service_(), socket_(io_service_), write_in_progress_(false) +{} + +UDPBoard::~UDPBoard() +{ + MutexLock read_lock(read_mutex_); + MutexLock write_lock(write_mutex_); + + io_service_.stop(); + socket_.close(); + + if (io_thread_.joinable()) { io_thread_.join(); } +} + +void UDPBoard::set_ports(std::string bind_host, uint16_t bind_port, std::string remote_host, + uint16_t remote_port) +{ + bind_host_ = std::move(bind_host); + bind_port_ = bind_port; + remote_host_ = std::move(remote_host); + remote_port_ = remote_port; +} + +void UDPBoard::serial_init(uint32_t baud_rate, uint32_t dev) +{ + // can throw an uncaught boost::system::system_error exception + (void) dev; + + udp::resolver resolver(io_service_); + + bind_endpoint_ = *resolver.resolve({udp::v4(), bind_host_, ""}); + bind_endpoint_.port(bind_port_); + + remote_endpoint_ = *resolver.resolve({udp::v4(), remote_host_, ""}); + remote_endpoint_.port(remote_port_); + + socket_.open(udp::v4()); + socket_.bind(bind_endpoint_); + + socket_.set_option(udp::socket::reuse_address(true)); + socket_.set_option(udp::socket::send_buffer_size(1000 * MAVLINK_MAX_PACKET_LEN)); + socket_.set_option(udp::socket::receive_buffer_size(1000 * MAVLINK_MAX_PACKET_LEN)); + + async_read(); + io_thread_ = boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_)); +} + +void UDPBoard::serial_flush() {} + +void UDPBoard::serial_write(const uint8_t * src, size_t len) +{ + auto buffer = new Buffer(src, len); + + { + MutexLock lock(write_mutex_); + write_queue_.push_back(buffer); + } + + async_write(true); +} + +uint16_t UDPBoard::serial_bytes_available() +{ + MutexLock lock(read_mutex_); + return !read_queue_.empty(); //! \todo This should return a number, not a bool +} + +uint8_t UDPBoard::serial_read() +{ + MutexLock lock(read_mutex_); + + if (read_queue_.empty()) { return 0; } + + Buffer * buffer = read_queue_.front(); + uint8_t byte = buffer->consume_byte(); + + if (buffer->empty()) { + read_queue_.pop_front(); + delete buffer; + } + return byte; +} + +void UDPBoard::async_read() +{ + if (!socket_.is_open()) { return; } + + MutexLock lock(read_mutex_); + socket_.async_receive_from( + boost::asio::buffer(read_buffer_, MAVLINK_MAX_PACKET_LEN), remote_endpoint_, + boost::bind(&UDPBoard::async_read_end, this, boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); +} + +void UDPBoard::async_read_end(const boost::system::error_code & error, size_t bytes_transferred) +{ + if (!error) { + MutexLock lock(read_mutex_); + read_queue_.push_back(new Buffer(read_buffer_, bytes_transferred)); + } + async_read(); +} + +void UDPBoard::async_write(bool check_write_state) +{ + if (check_write_state && write_in_progress_) { return; } + + MutexLock lock(write_mutex_); + if (write_queue_.empty()) { return; } + + write_in_progress_ = true; + Buffer * buffer = write_queue_.front(); + socket_.async_send_to(boost::asio::buffer(buffer->dpos(), buffer->nbytes()), remote_endpoint_, + boost::bind(&UDPBoard::async_write_end, this, + boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); +} + +void UDPBoard::async_write_end(const boost::system::error_code & error, size_t bytes_transferred) +{ + if (!error) { + MutexLock lock(write_mutex_); + + if (write_queue_.empty()) { + write_in_progress_ = false; + return; + } + + Buffer * buffer = write_queue_.front(); + buffer->pos += bytes_transferred; + if (buffer->empty()) { + write_queue_.pop_front(); + delete buffer; + } + + if (write_queue_.empty()) { + write_in_progress_ = false; + } else { + async_write(false); + } + } +} + +} // namespace rosflight_firmware diff --git a/boards/udp/udp_board.hpp b/boards/udp/udp_board.hpp new file mode 100644 index 00000000..9ca6b046 --- /dev/null +++ b/boards/udp/udp_board.hpp @@ -0,0 +1,123 @@ +/* + * Copyright (c) 2017 Daniel Koch, BYU MAGICC Lab. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * \file udp_board.h + * \author Daniel Koch + */ + +#ifndef ROSFLIGHT_FIRMWARE_UDP_BOARD_H +#define ROSFLIGHT_FIRMWARE_UDP_BOARD_H + +#include +#include + +#include +#include + +#include "board.h" +#include "mavlink/mavlink.h" + +namespace rosflight_firmware +{ +class UDPBoard : public Board +{ +public: + explicit UDPBoard(std::string bind_host = "localhost", uint16_t bind_port = 14525, + std::string remote_host = "localhost", uint16_t remote_port = 14520); + ~UDPBoard(); + + void serial_init(uint32_t baud_rate, uint32_t dev) override; + void serial_write(const uint8_t * src, size_t len) override; + uint16_t serial_bytes_available() override; + uint8_t serial_read() override; + void serial_flush() override; + + void set_ports(std::string bind_host, uint16_t bind_port, std::string remote_host, + uint16_t remote_port); + +private: + struct Buffer + { + uint8_t data[MAVLINK_MAX_PACKET_LEN] = {0}; + size_t len; + size_t pos; + + Buffer() : len(0), pos(0) {} + + Buffer(const uint8_t * src, size_t length) : len(length), pos(0) + { + assert(length <= MAVLINK_MAX_PACKET_LEN); //! \todo Do something less catastrophic here + memcpy(data, src, length); + } + + const uint8_t * dpos() const { return data + pos; } + size_t nbytes() const { return len - pos; } + void add_byte(uint8_t byte) { data[len++] = byte; } + uint8_t consume_byte() { return data[pos++]; } + bool empty() const { return pos >= len; } + bool full() const { return len >= MAVLINK_MAX_PACKET_LEN; } + }; + + typedef boost::lock_guard MutexLock; + + void async_read(); + void async_read_end(const boost::system::error_code & error, size_t bytes_transferred); + + void async_write(bool check_write_state); + void async_write_end(const boost::system::error_code & error, size_t bytes_transferred); + + std::string bind_host_; + uint16_t bind_port_; + + std::string remote_host_; + uint16_t remote_port_; + + boost::thread io_thread_; + boost::recursive_mutex write_mutex_; + boost::recursive_mutex read_mutex_; + + boost::asio::io_service io_service_; + + boost::asio::ip::udp::socket socket_; + boost::asio::ip::udp::endpoint bind_endpoint_; + boost::asio::ip::udp::endpoint remote_endpoint_; + + uint8_t read_buffer_[MAVLINK_MAX_PACKET_LEN] = {0}; + std::list read_queue_; + + std::list write_queue_; + bool write_in_progress_; +}; + +} // namespace rosflight_firmware + +#endif // ROSFLIGHT_FIRMWARE_UDP_BOARD_H