JaiaBot  1.19.0
JaiaBot micro-AUV software
serial.h
Go to the documentation of this file.
1 
2 #ifndef JAIABOT_LORA_SERIAL_H
3 #define JAIABOT_LORA_SERIAL_H
4 
5 #include <boost/asio.hpp>
6 #include <string>
7 
8 #include "goby/middleware/io/detail/io_interface.h" // for PubSubLayer
9 #include "goby/middleware/io/detail/serial_interface.h" // for SerialThread
10 #include "jaiabot/messages/nanopb/feather.pb.h"
11 
12 #include "crc16.h"
13 
14 namespace goby
15 {
16 namespace middleware
17 {
18 class Group;
19 }
20 } // namespace goby
21 namespace goby
22 {
23 namespace middleware
24 {
25 namespace protobuf
26 {
27 class SerialConfig;
28 }
29 } // namespace middleware
30 } // namespace goby
31 
32 namespace jaiabot
33 {
34 namespace lora
35 {
36 constexpr const char* SERIAL_MAGIC = "JAIA";
37 constexpr int SERIAL_MAGIC_BYTES = 4;
38 constexpr int SIZE_BYTES = 2;
39 constexpr int BITS_IN_BYTE = 8;
40 constexpr auto SERIAL_MAX_SIZE =
41  SERIAL_MAGIC_BYTES + SIZE_BYTES + (1ul << (SIZE_BYTES * BITS_IN_BYTE));
42 constexpr auto CRC_SIZE = 4;
43 
47 template <const goby::middleware::Group& line_in_group,
48  const goby::middleware::Group& line_out_group,
49  goby::middleware::io::PubSubLayer publish_layer =
50  goby::middleware::io::PubSubLayer::INTERPROCESS,
51  goby::middleware::io::PubSubLayer subscribe_layer =
52  goby::middleware::io::PubSubLayer::INTERTHREAD,
53  template <class> class ThreadType = goby::middleware::SimpleThread,
54  bool use_indexed_groups = false>
55 class SerialThreadLoRaFeather
56  : public goby::middleware::io::detail::SerialThread<line_in_group, line_out_group,
57  publish_layer, subscribe_layer, ThreadType,
58  use_indexed_groups>
59 {
60  using Base =
61  goby::middleware::io::detail::SerialThread<line_in_group, line_out_group, publish_layer,
62  subscribe_layer, ThreadType, use_indexed_groups>;
63 
64  public:
65  SerialThreadLoRaFeather(const goby::middleware::protobuf::SerialConfig& config, int index = -1)
66  : Base(config, index)
67  {
68  }
69 
70  ~SerialThreadLoRaFeather() {}
71 
72  private:
73  void async_read() override;
74 
75  void read_first_byte();
76  void read_magic();
77  void read_size();
78  void read_body();
79 
80  private:
81  std::array<char, SERIAL_MAX_SIZE> buffer_;
82  char* buffer_write_ptr_{buffer_.data()};
83  std::uint16_t message_size_{0};
84 };
85 
86 template <typename ProtobufMessage>
87 ProtobufMessage parse(const goby::middleware::protobuf::IOData& io)
88 {
89  auto& data = io.data();
90  ProtobufMessage pb_msg;
91  constexpr auto prefix_size = jaiabot::lora::SERIAL_MAGIC_BYTES + jaiabot::lora::SIZE_BYTES;
92 
93  // Check CRC32
94  // auto calculated_crc = crc32(&data[0] + prefix_size, data.size() - prefix_size - CRC_SIZE);
95  // auto crc = *((uint32_t *) (&data[0] + data.size() - CRC_SIZE));
96 
97  // if (calculated_crc != crc) {
98  // throw std::string("CRC mismatch!");
99  // }
100 
101  pb_msg.ParseFromArray(&data[0] + prefix_size, data.size() - prefix_size);
102  return pb_msg;
103 }
104 
105 template <typename ProtobufMessage>
106 std::shared_ptr<goby::middleware::protobuf::IOData> serialize(const ProtobufMessage& pb_msg)
107 {
108  auto io = std::make_shared<goby::middleware::protobuf::IOData>();
109 
110  std::string pb_encoded = pb_msg.SerializeAsString();
111 
112  std::uint16_t size = pb_encoded.size();
113  std::string size_str = {static_cast<char>((size >> jaiabot::lora::BITS_IN_BYTE) & 0xFF),
114  static_cast<char>(size & 0xFF)};
115 
116  auto header_and_data = jaiabot::lora::SERIAL_MAGIC + size_str + pb_encoded;
117 
118  // Add CRC32
119  auto crc = fletcher16(&pb_encoded[0], pb_encoded.size());
120  std::string crc_data = std::string((const char*)&crc, sizeof(crc));
121 
122  // FAKE NOISE
123  // header_and_data[std::rand() % header_and_data.size()] = std::rand() % 256;
124 
125  io->set_data(header_and_data + crc_data);
126 
127  return io;
128 }
129 
130 } // namespace lora
131 } // namespace jaiabot
132 
133 template <const goby::middleware::Group& line_in_group,
134  const goby::middleware::Group& line_out_group,
135  goby::middleware::io::PubSubLayer publish_layer,
136  goby::middleware::io::PubSubLayer subscribe_layer, template <class> class ThreadType,
137  bool use_indexed_groups>
138 void jaiabot::lora::SerialThreadLoRaFeather<line_in_group, line_out_group, publish_layer,
139  subscribe_layer, ThreadType,
140  use_indexed_groups>::async_read()
141 {
142  buffer_write_ptr_ = buffer_.data();
143  read_first_byte();
144 }
145 
146 template <const goby::middleware::Group& line_in_group,
147  const goby::middleware::Group& line_out_group,
148  goby::middleware::io::PubSubLayer publish_layer,
149  goby::middleware::io::PubSubLayer subscribe_layer, template <class> class ThreadType,
150  bool use_indexed_groups>
151 void jaiabot::lora::SerialThreadLoRaFeather<line_in_group, line_out_group, publish_layer,
152  subscribe_layer, ThreadType,
153  use_indexed_groups>::read_first_byte()
154 {
155  boost::asio::async_read(
156  this->mutable_serial_port(),
157  boost::asio::buffer(buffer_write_ptr_,
158  buffer_.size() - (buffer_write_ptr_ - buffer_.data())),
159  boost::asio::transfer_exactly(1),
160  [this](const boost::system::error_code& ec, std::size_t bytes_transferred) {
161  if (!ec && bytes_transferred > 0)
162  {
163  if (buffer_[0] != SERIAL_MAGIC[0])
164  {
165  goby::glog.is_warn() &&
166  goby::glog << "Invalid first byte, expected: " << SERIAL_MAGIC[0]
167  << ", received: " << buffer_[0] << std::endl;
168  this->async_read();
169  }
170  else
171  {
172  buffer_write_ptr_ += bytes_transferred;
173  read_magic();
174  }
175  }
176  else
177  {
178  this->handle_read_error(ec);
179  }
180  });
181 }
182 
183 template <const goby::middleware::Group& line_in_group,
184  const goby::middleware::Group& line_out_group,
185  goby::middleware::io::PubSubLayer publish_layer,
186  goby::middleware::io::PubSubLayer subscribe_layer, template <class> class ThreadType,
187  bool use_indexed_groups>
188 void jaiabot::lora::SerialThreadLoRaFeather<line_in_group, line_out_group, publish_layer,
189  subscribe_layer, ThreadType,
190  use_indexed_groups>::read_magic()
191 {
192  boost::asio::async_read(
193  this->mutable_serial_port(),
194  boost::asio::buffer(buffer_write_ptr_,
195  buffer_.size() - (buffer_write_ptr_ - buffer_.data())),
196  boost::asio::transfer_exactly(SERIAL_MAGIC_BYTES - 1),
197  [this](const boost::system::error_code& ec, std::size_t bytes_transferred) {
198  if (!ec && bytes_transferred > 0)
199  {
200  if (memcmp(buffer_.data(), SERIAL_MAGIC, SERIAL_MAGIC_BYTES) != 0)
201  {
202  goby::glog.is_warn() &&
203  goby::glog
204  << "Invalid magic word, expected: " << SERIAL_MAGIC << ", received: "
205  << std::string(buffer_.data(), buffer_.data() + SERIAL_MAGIC_BYTES)
206  << std::endl;
207  this->async_read();
208  }
209  else
210  {
211  buffer_write_ptr_ += bytes_transferred;
212  read_size();
213  }
214  }
215  else
216  {
217  this->handle_read_error(ec);
218  }
219  });
220 }
221 
222 template <const goby::middleware::Group& line_in_group,
223  const goby::middleware::Group& line_out_group,
224  goby::middleware::io::PubSubLayer publish_layer,
225  goby::middleware::io::PubSubLayer subscribe_layer, template <class> class ThreadType,
226  bool use_indexed_groups>
227 void jaiabot::lora::SerialThreadLoRaFeather<line_in_group, line_out_group, publish_layer,
228  subscribe_layer, ThreadType,
229  use_indexed_groups>::read_size()
230 {
231  boost::asio::async_read(
232  this->mutable_serial_port(),
233  boost::asio::buffer(buffer_write_ptr_,
234  buffer_.size() - (buffer_write_ptr_ - buffer_.data())),
235  boost::asio::transfer_exactly(SIZE_BYTES),
236  [this](const boost::system::error_code& ec, std::size_t bytes_transferred) {
237  if (!ec && bytes_transferred > 0)
238  {
239  message_size_ = 0;
240  message_size_ |= buffer_[SERIAL_MAGIC_BYTES];
241  message_size_ <<= BITS_IN_BYTE;
242  message_size_ |= buffer_[SERIAL_MAGIC_BYTES + 1];
243  if (message_size_ > jaiabot_protobuf_LoRaMessage_size)
244  {
245  goby::glog.is_warn() && goby::glog
246  << "Reported message size is larger than Protobuf "
247  "LoraMessage maximum size. Reported:"
248  << message_size_ << ", expected max: "
249  << jaiabot_protobuf_LoRaMessage_size << std::endl;
250 
251  this->async_read();
252  }
253  else
254  {
255  buffer_write_ptr_ += bytes_transferred;
256  read_body();
257  }
258  }
259  else
260  {
261  this->handle_read_error(ec);
262  }
263  });
264 }
265 
266 template <const goby::middleware::Group& line_in_group,
267  const goby::middleware::Group& line_out_group,
268  goby::middleware::io::PubSubLayer publish_layer,
269  goby::middleware::io::PubSubLayer subscribe_layer, template <class> class ThreadType,
270  bool use_indexed_groups>
271 void jaiabot::lora::SerialThreadLoRaFeather<line_in_group, line_out_group, publish_layer,
272  subscribe_layer, ThreadType,
273  use_indexed_groups>::read_body()
274 {
275  boost::asio::async_read(
276  this->mutable_serial_port(),
277  boost::asio::buffer(buffer_write_ptr_,
278  buffer_.size() - (buffer_write_ptr_ - buffer_.data())),
279  boost::asio::transfer_exactly(message_size_),
280  [this](const boost::system::error_code& ec, std::size_t bytes_transferred) {
281  if (!ec && bytes_transferred > 0)
282  {
283  buffer_write_ptr_ += bytes_transferred;
284  auto io_msg = std::make_shared<goby::middleware::protobuf::IOData>();
285  io_msg->set_data(std::string(buffer_.data(), buffer_write_ptr_));
286  this->handle_read_success(buffer_write_ptr_ - buffer_.data(), io_msg);
287  this->async_read();
288  }
289  else
290  {
291  this->handle_read_error(ec);
292  }
293  });
294 }
295 
296 #endif
jaiabot::lora::SerialThreadLoRaFeather::~SerialThreadLoRaFeather
~SerialThreadLoRaFeather()
Definition: serial.h:70
goby
Definition: udp_driver.h:59
crc16.h
jaiabot::udp::protobuf::config
extern ::google::protobuf::internal::ExtensionIdentifier< ::goby::acomms::protobuf::DriverConfig, ::google::protobuf::internal::MessageTypeTraits< ::jaiabot::udp::protobuf::Config >, 11, false > config
Definition: udp_driver.pb.h:566
jaiabot::lora::SerialThreadLoRaFeather::SerialThreadLoRaFeather
SerialThreadLoRaFeather(const goby::middleware::protobuf::SerialConfig &config, int index=-1)
Definition: serial.h:65
feather.pb.h
jaiabot::lora::CRC_SIZE
constexpr auto CRC_SIZE
Definition: serial.h:42
jaiabot::lora::SerialThreadLoRaFeather
Reads/Writes LoRa Adafruit feather message packages from/to serial port.
Definition: serial.h:55
jaiabot::lora::SERIAL_MAGIC
constexpr const char * SERIAL_MAGIC
Definition: serial.h:36
jaiabot::lora::SERIAL_MAGIC_BYTES
constexpr int SERIAL_MAGIC_BYTES
Definition: serial.h:37
jaiabot
Definition: config.pb.h:56
fletcher16
uint16_t fletcher16(const void *input_str, size_t num_bytes)
Definition: crc16.h:98
jaiabot::lora::BITS_IN_BYTE
constexpr int BITS_IN_BYTE
Definition: serial.h:39
jaiabot::lora::parse
ProtobufMessage parse(const goby::middleware::protobuf::IOData &io)
Definition: serial.h:87
jaiabot::lora::SIZE_BYTES
constexpr int SIZE_BYTES
Definition: serial.h:38
jaiabot::lora::serialize
std::shared_ptr< goby::middleware::protobuf::IOData > serialize(const ProtobufMessage &pb_msg)
Definition: serial.h:106
jaiabot::lora::SERIAL_MAX_SIZE
constexpr auto SERIAL_MAX_SIZE
Definition: serial.h:40