Added udp_board.cpp files from ROS rosflight_firmware package

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 <iostream>
+#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

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 <list>
+#include <string>
+
+#include <boost/asio.hpp>
+#include <boost/thread.hpp>
+
+#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<boost::recursive_mutex> 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<Buffer *> read_queue_;
+
+  std::list<Buffer *> write_queue_;
+  bool write_in_progress_;
+};
+
+} // namespace rosflight_firmware
+
+#endif // ROSFLIGHT_FIRMWARE_UDP_BOARD_H