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