Goby3  3.1.5a
2024.05.23
waveglider_sv2_serial_client.h
Go to the documentation of this file.
1 // Copyright 2017-2023:
2 // GobySoft, LLC (2013-)
3 // Community contributors (see AUTHORS file)
4 // File authors:
5 // Toby Schneider <toby@gobysoft.org>
6 //
7 //
8 // This file is part of the Goby Underwater Autonomy Project Libraries
9 // ("The Goby Libraries").
10 //
11 // The Goby Libraries are free software: you can redistribute them and/or modify
12 // them under the terms of the GNU Lesser General Public License as published by
13 // the Free Software Foundation, either version 2.1 of the License, or
14 // (at your option) any later version.
15 //
16 // The Goby Libraries are distributed in the hope that they will be useful,
17 // but WITHOUT ANY WARRANTY; without even the implied warranty of
18 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 // GNU Lesser General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public License
22 // along with Goby. If not, see <http://www.gnu.org/licenses/>.
23 
24 #ifndef GOBY_MIDDLEWARE_FRONTSEAT_WAVEGLIDER_WAVEGLIDER_SV2_SERIAL_CLIENT_H
25 #define GOBY_MIDDLEWARE_FRONTSEAT_WAVEGLIDER_WAVEGLIDER_SV2_SERIAL_CLIENT_H
26 
27 #include <memory>
28 
29 #include "goby/util/asio_compat.h"
30 #include <boost/asio.hpp>
31 #include <utility>
32 
33 #include <dccl/binary.h>
34 
35 namespace goby
36 {
37 namespace middleware
38 {
39 namespace frontseat
40 {
41 class SV2SerialConnection : public std::enable_shared_from_this<SV2SerialConnection>
42 {
43  public:
44  static std::shared_ptr<SV2SerialConnection> create(boost::asio::io_context& io_context,
45  std::string name, int baud = 115200)
46  {
47  return std::shared_ptr<SV2SerialConnection>(
48  new SV2SerialConnection(io_context, std::move(name), baud));
49  }
50 
51  boost::asio::serial_port& socket() { return socket_; }
52 
53  void start() { read_start(); }
54 
55  void close() { socket_.close(); }
56 
57  void read_start()
58  {
59  std::fill(message_.begin(), message_.end(), 0);
60  last_escape_ = 0;
61 
62  boost::asio::async_read(
63  socket_, buffer_, boost::asio::transfer_exactly(1),
64  boost::bind(&SV2SerialConnection::handle_read, this, boost::placeholders::_1, boost::placeholders::_2, PART_MAGIC));
65  }
66 
67  void write_start(std::string data)
68  {
69  add_escapes(&data);
70  boost::asio::async_write(socket_, boost::asio::buffer(data),
71  boost::bind(&SV2SerialConnection::handle_write, this, boost::placeholders::_1, boost::placeholders::_2));
72  }
73 
74  ~SV2SerialConnection() = default;
75 
76  boost::signals2::signal<void(const std::string& message)> message_signal;
77 
78  private:
79  enum MessagePart
80  {
81  PART_MAGIC,
82  PART_HEADER,
83  PART_COMPLETE
84  };
85 
86  SV2SerialConnection(boost::asio::io_context& io_context, const std::string& name, int baud)
87  : socket_(io_context),
88  message_(SV2_MAX_SIZE * 2) // leave room for escape chars (in theory, every byte)
89  {
90  using goby::glog;
92  try
93  {
94  socket_.open(name);
95  }
96  catch (std::exception& e)
97  {
98  glog.is(DIE) && glog << "Failed to open serial port: " << name << std::endl;
99  }
100 
101  socket_.set_option(boost::asio::serial_port_base::baud_rate(baud));
102 
103  // no flow control
104  socket_.set_option(boost::asio::serial_port_base::flow_control(
106 
107  // 8N1
108  socket_.set_option(boost::asio::serial_port_base::character_size(8));
109  socket_.set_option(
110  boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
111  socket_.set_option(boost::asio::serial_port_base::stop_bits(
112  boost::asio::serial_port_base::stop_bits::one));
113  }
114 
115  void handle_write(const boost::system::error_code& error, size_t /*bytes_transferred*/)
116  {
117  if (error)
118  {
119  using goby::glog;
121  glog.is(WARN) && glog << "Error writing to serial connection: " << error << std::endl;
122  }
123  }
124 
125  void handle_read(const boost::system::error_code& error, size_t bytes_transferred,
126  MessagePart part)
127  {
128  using goby::glog;
132 
133  if (!error)
134  {
135  std::istream istrm(&buffer_);
136 
137  switch (part)
138  {
139  case PART_MAGIC:
140  {
141  if ((istrm.peek() & 0xFF) != SV2_MAGIC_BYTE)
142  {
143  glog.is(DEBUG2) && glog << std::hex << istrm.peek() << std::dec
144  << ": Not magic byte, continuing to read."
145  << std::endl;
146  read_start();
147  buffer_.consume(bytes_transferred);
148  }
149  else
150  {
151  glog.is(DEBUG2) && glog << "Received magic byte. Starting read of "
152  << SV2_HEADER_SIZE << " bytes" << std::endl;
153  message_insert_ = &message_[0];
154  istrm.read(message_insert_, bytes_transferred);
155  message_insert_ += bytes_transferred;
156 
157  // read the header
158  boost::asio::async_read(socket_, buffer_,
159  boost::asio::transfer_exactly(SV2_HEADER_SIZE),
160  boost::bind(&SV2SerialConnection::handle_read, this,
161  boost::placeholders::_1, boost::placeholders::_2, PART_HEADER));
162  }
163  break;
164  }
165 
166  case PART_HEADER:
167  {
168  istrm.read(message_insert_, bytes_transferred);
169  message_insert_ += bytes_transferred;
170 
171  if (!check_escapes(part))
172  return;
173  remove_escapes();
174 
175  message_size_ = message_[1] & 0xFF;
176  message_size_ |= (message_[2] & 0xFF) << 8;
177 
178  glog.is(DEBUG2) &&
179  glog << "Received header: "
180  << dccl::hex_encode(std::string(
181  message_.begin(), message_.begin() + SV2_HEADER_SIZE + 1))
182  << ", size of " << message_size_ << std::endl;
183 
184  // read the rest of the message
185  boost::asio::async_read(
186  socket_, buffer_,
187  boost::asio::transfer_exactly(message_size_ - SV2_HEADER_SIZE),
188  boost::bind(&SV2SerialConnection::handle_read, this, boost::placeholders::_1, boost::placeholders::_2,
189  PART_COMPLETE));
190  break;
191  }
192 
193  case PART_COMPLETE:
194  {
195  istrm.read(message_insert_, bytes_transferred);
196  message_insert_ += bytes_transferred;
197 
198  if (!check_escapes(part))
199  return;
200  remove_escapes();
201 
202  glog.is(DEBUG1) &&
203  glog << "Read message: "
204  << dccl::hex_encode(std::string(message_.begin(),
205  message_.begin() + message_size_ + 1))
206  << std::endl;
208  std::string(message_.begin(), message_.begin() + message_size_ + 1));
209  read_start();
210 
211  break;
212  }
213  }
214  }
215  else
216  {
217  if (error == boost::asio::error::eof)
218  {
219  glog.is(DEBUG1) && glog << "Connection reached EOF" << std::endl;
220  }
221  else if (error == boost::asio::error::operation_aborted)
222  {
223  glog.is(DEBUG1) && glog << "Read operation aborted (socket closed)" << std::endl;
224  }
225  else
226  {
227  glog.is(WARN) && glog << "Error reading from serial connection: " << error
228  << std::endl;
229  }
230  }
231  }
232 
233  bool check_escapes(MessagePart part)
234  {
235  using goby::glog;
238  int escape = std::count(message_.begin(), message_.end(), SV2_ESCAPE);
239  if (escape != last_escape_)
240  {
241  boost::asio::async_read(
242  socket_, buffer_, boost::asio::transfer_exactly(escape - last_escape_),
243  boost::bind(&SV2SerialConnection::handle_read, this, boost::placeholders::_1, boost::placeholders::_2, part));
244  glog.is(DEBUG1) && glog << "Reading " << escape - last_escape_
245  << " more bytes because of escape characters." << std::endl;
246 
247  last_escape_ = escape;
248  return false;
249  }
250  else
251  {
252  return true;
253  }
254  }
255 
256  void remove_escapes()
257  {
258  if (last_escape_ == 0)
259  return;
260 
261  // remove escape characters - order matters (or else 0x7d5d5e would become 0x7e)
262  int before_size = message_.size();
263  boost::replace_all(message_, std::string(1, SV2_ESCAPE) + std::string(1, 0x5E),
264  std::string(1, SV2_MAGIC_BYTE));
265  boost::replace_all(message_, std::string(1, SV2_ESCAPE) + std::string(1, 0x5D),
266  std::string(1, SV2_ESCAPE));
267  int after_size = message_.size();
268 
269  message_insert_ -= (before_size - after_size);
270  message_.resize(before_size);
271  last_escape_ = std::count(message_.begin(), message_.end(), SV2_ESCAPE);
272  }
273 
274  void add_escapes(std::string* message)
275  {
276  // add escape characters - order matters (or else 0x7e becomes 0x7d5d5e)
277  boost::replace_all(*message, std::string(1, SV2_ESCAPE),
278  std::string(1, SV2_ESCAPE) + std::string(1, 0x5D));
279  boost::replace_all(*message, std::string(1, SV2_MAGIC_BYTE),
280  std::string(1, SV2_ESCAPE) + std::string(1, 0x5E));
281  // restore the actual start of frame
282  boost::replace_first(*message, std::string(1, SV2_ESCAPE) + std::string(1, 0x5E),
283  std::string(1, SV2_MAGIC_BYTE));
284  }
285 
286  private:
287  boost::asio::serial_port socket_;
288  boost::asio::streambuf buffer_;
289  std::vector<char> message_;
290  char* message_insert_;
291  int message_size_;
292  int last_escape_;
293 
294  enum
295  {
296  SV2_MAGIC_BYTE = 0x7e,
297  SV2_ESCAPE = 0x7d
298  };
299  enum
300  {
301  SV2_HEADER_SIZE = 10
302  }; // not including magic byte
303  enum
304  {
305  SV2_MAX_SIZE = 557
306  };
307 };
308 } // namespace frontseat
309 } // namespace middleware
310 } // namespace goby
311 
312 #endif
goby::middleware::frontseat::SV2SerialConnection::message_signal
boost::signals2::signal< void(const std::string &message)> message_signal
Definition: waveglider_sv2_serial_client.h:76
goby::util::FlexOstream::is
bool is(goby::util::logger::Verbosity verbosity)
goby
The global namespace for the Goby project.
Definition: acomms_constants.h:33
goby::util::logger::DEBUG2
@ DEBUG2
Definition: flex_ostreambuf.h:78
goby::middleware::frontseat::SV2SerialConnection::read_start
void read_start()
Definition: waveglider_sv2_serial_client.h:57
goby::util::logger::WARN
@ WARN
Definition: flex_ostreambuf.h:74
goby::util::e
constexpr T e
Definition: constants.h:35
detail::escape
StringType escape(StringType s)
string escaping as described in RFC 6901 (Sect. 4)
Definition: json.hpp:2974
goby::util::logger_lock::none
@ none
Definition: flex_ostreambuf.h:61
detail::void
j template void())
Definition: json.hpp:4822
io_service
boost::asio::io_context
io_service io_context
Definition: asio_compat.h:44
goby::middleware::frontseat::SV2SerialConnection
Definition: waveglider_sv2_serial_client.h:41
goby::middleware::frontseat::SV2SerialConnection::close
void close()
Definition: waveglider_sv2_serial_client.h:55
goby::util::logger::DIE
@ DIE
Definition: flex_ostreambuf.h:80
goby::middleware::frontseat::SV2SerialConnection::~SV2SerialConnection
~SV2SerialConnection()=default
goby::middleware::frontseat::SV2SerialConnection::start
void start()
Definition: waveglider_sv2_serial_client.h:53
asio_compat.h
goby::acomms::bind
void bind(ModemDriverBase &driver, QueueManager &queue_manager)
binds the driver link-layer callbacks to the QueueManager
Definition: bind.h:45
goby::middleware::frontseat::SV2SerialConnection::socket
boost::asio::serial_port & socket()
Definition: waveglider_sv2_serial_client.h:51
goby::util::logger::DEBUG1
@ DEBUG1
Definition: flex_ostreambuf.h:77
goby::util::hex_encode
void hex_encode(const std::string &in, std::string *out, bool upper_case=false)
Encodes a (little-endian) hexadecimal string from a byte string. Index 0 of in is written to index 0 ...
Definition: binary.h:94
goby::glog
util::FlexOstream glog
Access the Goby logger through this object.
detail::cbor_tag_handler_t::error
@ error
throw a parse_error exception in case of a tag
goby::middleware::frontseat::SV2SerialConnection::write_start
void write_start(std::string data)
Definition: waveglider_sv2_serial_client.h:67
goby::middleware::frontseat::SV2SerialConnection::create
static std::shared_ptr< SV2SerialConnection > create(boost::asio::io_context &io_context, std::string name, int baud=115200)
Definition: waveglider_sv2_serial_client.h:44