udp_board.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017 Daniel Koch, BYU MAGICC Lab.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright notice, this
9  * list of conditions and the following disclaimer.
10  *
11  * * Redistributions in binary form must reproduce the above copyright notice,
12  * this list of conditions and the following disclaimer in the documentation
13  * and/or other materials provided with the distribution.
14  *
15  * * Neither the name of the copyright holder nor the names of its
16  * contributors may be used to endorse or promote products derived from
17  * this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
38 #include <iostream>
39 
40 using boost::asio::ip::udp;
41 
42 namespace rosflight_firmware
43 {
44 
45 UDPBoard::UDPBoard(std::string bind_host, uint16_t bind_port, std::string remote_host, uint16_t remote_port) :
46  bind_host_(bind_host),
47  bind_port_(bind_port),
48  remote_host_(remote_host),
49  remote_port_(remote_port),
50  io_service_(),
51  socket_(io_service_),
52  write_in_progress_(false)
53 {
54 }
55 
57 {
58  MutexLock read_lock(read_mutex_);
59  MutexLock write_lock(write_mutex_);
60 
61  io_service_.stop();
62  socket_.close();
63 
64  if (io_thread_.joinable())
65  io_thread_.join();
66 }
67 
68 void UDPBoard::set_ports(std::string bind_host, uint16_t bind_port, std::string remote_host, uint16_t remote_port)
69 {
70  bind_host_ = bind_host;
71  bind_port_ = bind_port;
72  remote_host_ = remote_host;
73  remote_port_ = remote_port;
74 }
75 
76 void UDPBoard::serial_init(uint32_t baud_rate, uint32_t dev)
77 {
78  // can throw an uncaught boost::system::system_error exception
79  (void) dev;
80 
81  udp::resolver resolver(io_service_);
82 
83  bind_endpoint_ = *resolver.resolve({udp::v4(), bind_host_, ""});
85 
86  remote_endpoint_ = *resolver.resolve({udp::v4(), remote_host_, ""});
88 
89  socket_.open(udp::v4());
90  socket_.bind(bind_endpoint_);
91 
92  socket_.set_option(udp::socket::reuse_address(true));
93  socket_.set_option(udp::socket::send_buffer_size(1000*MAVLINK_MAX_PACKET_LEN));
94  socket_.set_option(udp::socket::receive_buffer_size(1000*MAVLINK_MAX_PACKET_LEN));
95 
96  async_read();
97  io_thread_ = boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_));
98 }
99 
101 {}
102 
103 void UDPBoard::serial_write(const uint8_t *src, size_t len)
104 {
105  Buffer *buffer = new Buffer(src, len);
106 
107  {
108  MutexLock lock(write_mutex_);
109  write_queue_.push_back(buffer);
110  }
111 
112  async_write(true);
113 }
114 
116 {
117  MutexLock lock(read_mutex_);
118  return !read_queue_.empty();
119 }
120 
122 {
123  MutexLock lock(read_mutex_);
124 
125  if (read_queue_.empty())
126  return 0;
127 
128  Buffer *buffer = read_queue_.front();
129  uint8_t byte = buffer->consume_byte();
130 
131  if (buffer->empty())
132  {
133  read_queue_.pop_front();
134  delete buffer;
135  }
136  return byte;
137 }
138 
140 {
141  if (!socket_.is_open()) return;
142 
143  MutexLock lock(read_mutex_);
146  boost::bind(&UDPBoard::async_read_end,
147  this,
149  boost::asio::placeholders::bytes_transferred));
150 }
151 
152 void UDPBoard::async_read_end(const boost::system::error_code &error, size_t bytes_transferred)
153 {
154  if (!error)
155  {
156  MutexLock lock(read_mutex_);
157  read_queue_.push_back(new Buffer(read_buffer_, bytes_transferred));
158  }
159  async_read();
160 }
161 
162 void UDPBoard::async_write(bool check_write_state)
163 {
164  if (check_write_state && write_in_progress_)
165  return;
166 
167  MutexLock lock(write_mutex_);
168  if (write_queue_.empty())
169  return;
170 
171  write_in_progress_ = true;
172  Buffer *buffer = write_queue_.front();
173  socket_.async_send_to(boost::asio::buffer(buffer->dpos(), buffer->nbytes()),
175  boost::bind(&UDPBoard::async_write_end,
176  this,
178  boost::asio::placeholders::bytes_transferred));
179 }
180 
181 void UDPBoard::async_write_end(const boost::system::error_code &error, size_t bytes_transferred)
182 {
183  if (!error)
184  {
185  MutexLock lock(write_mutex_);
186 
187  if (write_queue_.empty())
188  {
189  write_in_progress_ = false;
190  return;
191  }
192 
193  Buffer *buffer = write_queue_.front();
194  buffer->pos += bytes_transferred;
195  if (buffer->empty())
196  {
197  write_queue_.pop_front();
198  delete buffer;
199  }
200 
201  if (write_queue_.empty())
202  write_in_progress_ = false;
203  else
204  async_write(false);
205  }
206 }
207 
208 } // namespace rosflight_firmware
void serial_flush() override
Definition: udp_board.cpp:100
boost::asio::ip::udp::socket socket_
Definition: udp_board.h:112
std::list< Buffer * > write_queue_
Definition: udp_board.h:119
static volatile bool error
Definition: drv_i2c.c:92
void serial_init(uint32_t baud_rate, uint32_t dev) override
Definition: udp_board.cpp:76
void serial_write(const uint8_t *src, size_t len) override
Definition: udp_board.cpp:103
static uint8_t buffer[BMP280_DATA_FRAME_SIZE]
Definition: drv_bmp280.c:61
UDPBoard(std::string bind_host="localhost", uint16_t bind_port=14525, std::string remote_host="localhost", uint16_t remote_port=14520)
Definition: udp_board.cpp:45
boost::asio::ip::udp::endpoint bind_endpoint_
Definition: udp_board.h:113
boost::asio::ip::udp::endpoint remote_endpoint_
Definition: udp_board.h:114
void async_write(bool check_write_state)
Definition: udp_board.cpp:162
void async_read_end(const boost::system::error_code &error, size_t bytes_transferred)
Definition: udp_board.cpp:152
boost::lock_guard< boost::recursive_mutex > MutexLock
Definition: udp_board.h:92
boost::recursive_mutex write_mutex_
Definition: udp_board.h:107
uint16_t serial_bytes_available(void) override
Definition: udp_board.cpp:115
const uint8_t * dpos() const
Definition: udp_board.h:84
void set_ports(std::string bind_host, uint16_t bind_port, std::string remote_host, uint16_t remote_port)
Definition: udp_board.cpp:68
uint8_t serial_read(void) override
Definition: udp_board.cpp:121
boost::recursive_mutex read_mutex_
Definition: udp_board.h:108
boost::asio::io_service io_service_
Definition: udp_board.h:110
std::list< Buffer * > read_queue_
Definition: udp_board.h:117
void async_write_end(const boost::system::error_code &error, size_t bytes_transferred)
Definition: udp_board.cpp:181
uint8_t read_buffer_[MAVLINK_MAX_PACKET_LEN]
Definition: udp_board.h:116


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Oct 24 2019 03:17:20