JaiaBot 3.0.0
JaiaBot micro-AUV software
Loading...
Searching...
No Matches
serial_crc32.h
Go to the documentation of this file.
1
2#ifndef JAIABOT_SERIAL_CRC32_H
3#define JAIABOT_SERIAL_CRC32_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
12#include "jaiabot/crc/crc32.h"
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 serial
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 = 2048; // Can be increased, just for safety
43constexpr auto CRC_SIZE = 4;
44
45static const std::shared_ptr<goby::middleware::protobuf::IOData>
46encode_frame(const std::string& data)
47{
48 std::uint16_t size = data.length();
49 std::string size_str = {static_cast<char>((size >> jaiabot::serial::BITS_IN_BYTE) & 0xFF),
50 static_cast<char>(size & 0xFF)};
51
52 auto crc = calculate_crc32(data.c_str(), size);
53 char crc_data[4];
54 for (int i = 0; i < 4; i++) { crc_data[i] = ((char*)&crc)[3 - i]; }
55
56 auto io_data = std::make_shared<goby::middleware::protobuf::IOData>();
57 io_data->set_data(std::string(SERIAL_MAGIC, SERIAL_MAGIC_BYTES) + size_str +
58 string(crc_data, 4) + data);
59
60 return io_data;
61}
62
63static const std::string decode_frame(const std::string& frame_data)
64{
65 const auto HEADER_LENGTH = SERIAL_MAGIC_BYTES + SIZE_BYTES + CRC_SIZE;
66 auto size = frame_data.length() - HEADER_LENGTH;
67
68 if (size >= 0)
69 {
70 return frame_data.substr(HEADER_LENGTH, size);
71 }
72 else
73 {
74 return "";
75 }
76}
77
81template <const goby::middleware::Group& line_in_group,
82 const goby::middleware::Group& line_out_group,
83 goby::middleware::io::PubSubLayer publish_layer =
84 goby::middleware::io::PubSubLayer::INTERPROCESS,
85 goby::middleware::io::PubSubLayer subscribe_layer =
86 goby::middleware::io::PubSubLayer::INTERTHREAD,
87 template <class> class ThreadType = goby::zeromq::SimpleThread,
88 bool use_indexed_groups = false>
90 : public goby::middleware::io::detail::SerialThread<line_in_group, line_out_group,
91 publish_layer, subscribe_layer, ThreadType,
92 use_indexed_groups>
93{
94 using Base =
95 goby::middleware::io::detail::SerialThread<line_in_group, line_out_group, publish_layer,
96 subscribe_layer, ThreadType, use_indexed_groups>;
97
98 public:
99 SerialThreadCRC32(const goby::middleware::protobuf::SerialConfig& config, int index = -1)
100 : Base(config, index)
101 {
102 }
103
105
106 private:
107 void async_read() override
108 {
109 buffer_write_ptr_ = buffer_.data();
110 read_first_byte();
111 }
112
113 void read_first_byte()
114 {
115 boost::asio::async_read(
116 this->mutable_serial_port(),
117 boost::asio::buffer(buffer_write_ptr_,
118 buffer_.size() - (buffer_write_ptr_ - buffer_.data())),
119 boost::asio::transfer_exactly(1),
120 [this](const boost::system::error_code& ec, std::size_t bytes_transferred) {
121 if (!ec && bytes_transferred > 0)
122 {
123 if (buffer_[0] != SERIAL_MAGIC[0])
124 {
125 goby::glog.is_warn() &&
126 goby::glog << "Invalid first byte, expected: " << SERIAL_MAGIC[0]
127 << ", received: " << buffer_[0] << std::endl;
128 this->async_read();
129 }
130 else
131 {
132 buffer_write_ptr_ += bytes_transferred;
133 read_magic();
134 }
135 }
136 else
137 {
138 this->handle_read_error(ec);
139 }
140 });
141 }
142
143 void read_magic()
144 {
145 boost::asio::async_read(
146 this->mutable_serial_port(),
147 boost::asio::buffer(buffer_write_ptr_,
148 buffer_.size() - (buffer_write_ptr_ - buffer_.data())),
149 boost::asio::transfer_exactly(SERIAL_MAGIC_BYTES - 1),
150 [this](const boost::system::error_code& ec, std::size_t bytes_transferred) {
151 if (!ec && bytes_transferred > 0)
152 {
153 if (memcmp(buffer_.data(), SERIAL_MAGIC, SERIAL_MAGIC_BYTES) != 0)
154 {
155 goby::glog.is_warn() &&
156 goby::glog
157 << "Invalid magic word, expected: " << SERIAL_MAGIC
158 << ", received: "
159 << std::string(buffer_.data(), buffer_.data() + SERIAL_MAGIC_BYTES)
160 << std::endl;
161 this->async_read();
162 }
163 else
164 {
165 buffer_write_ptr_ += bytes_transferred;
166 read_size();
167 }
168 }
169 else
170 {
171 this->handle_read_error(ec);
172 }
173 });
174 }
175
176 void read_size()
177 {
178 boost::asio::async_read(
179 this->mutable_serial_port(),
180 boost::asio::buffer(buffer_write_ptr_,
181 buffer_.size() - (buffer_write_ptr_ - buffer_.data())),
182 boost::asio::transfer_exactly(SIZE_BYTES),
183 [this](const boost::system::error_code& ec, std::size_t bytes_transferred) {
184 if (!ec && bytes_transferred > 0)
185 {
186 message_size_ = 0;
187 message_size_ |= buffer_[SERIAL_MAGIC_BYTES];
188 message_size_ <<= BITS_IN_BYTE;
189 message_size_ |= buffer_[SERIAL_MAGIC_BYTES + 1];
190 if (message_size_ > SERIAL_MAX_SIZE)
191 {
192 goby::glog.is_warn() &&
193 goby::glog
194 << "Reported message size is larger than SERIAL_MAX_SIZE. Reported:"
195 << message_size_ << ", expected max: " << SERIAL_MAX_SIZE
196 << std::endl;
197
198 this->async_read();
199 }
200 else
201 {
202 buffer_write_ptr_ += bytes_transferred;
203 read_crc32();
204 }
205 }
206 else
207 {
208 this->handle_read_error(ec);
209 }
210 });
211 }
212
213 void read_crc32()
214 {
215 boost::asio::async_read(
216 this->mutable_serial_port(),
217 boost::asio::buffer(buffer_write_ptr_,
218 buffer_.size() - (buffer_write_ptr_ - buffer_.data())),
219 boost::asio::transfer_exactly(CRC_SIZE),
220 [this](const boost::system::error_code& ec, std::size_t bytes_transferred) {
221 if (!ec && bytes_transferred > 0)
222 {
223 crc32_ = 0;
224 char* ptr = reinterpret_cast<char*>(&crc32_);
225
226 // Manually construct the integer from the byte array
227 for (int i = 0; i < 4; i++) { ptr[3 - i] = buffer_write_ptr_[i]; }
228
229 buffer_write_ptr_ += bytes_transferred;
230 read_body();
231 }
232 else
233 {
234 this->handle_read_error(ec);
235 }
236 });
237 }
238
239 void read_body()
240 {
241 boost::asio::async_read(
242 this->mutable_serial_port(),
243 boost::asio::buffer(buffer_write_ptr_,
244 buffer_.size() - (buffer_write_ptr_ - buffer_.data())),
245 boost::asio::transfer_exactly(message_size_),
246 [this](const boost::system::error_code& ec, std::size_t bytes_transferred) {
247 if (!ec && bytes_transferred > 0)
248 {
249 auto actual_crc32 = calculate_crc32(buffer_write_ptr_, bytes_transferred);
250
251 if (crc32_ != actual_crc32)
252 {
253 goby::glog.is_warn() &&
254 goby::glog << "message_size_: " << message_size_
255 << ", bytes_transferred: " << bytes_transferred
256 << ", expected crc32: " << crc32_
257 << ", actual_crc32: " << actual_crc32 << std::endl;
258
259 goby::glog.is_warn() && goby::glog << "CRC32 failure. Expected: " << crc32_
260 << ", actual: " << actual_crc32
261 << std::endl;
262
263 this->async_read();
264 }
265
266 buffer_write_ptr_ += bytes_transferred;
267
268 auto io_msg = std::make_shared<goby::middleware::protobuf::IOData>();
269 io_msg->set_data(std::string(buffer_.data(), buffer_write_ptr_));
270 this->handle_read_success(buffer_write_ptr_ - buffer_.data(), io_msg);
271 this->async_read();
272 }
273 else
274 {
275 this->handle_read_error(ec);
276 }
277 });
278 }
279
280 private:
281 std::array<char, SERIAL_MAX_SIZE> buffer_;
282 char* buffer_write_ptr_{buffer_.data()};
283 std::uint16_t message_size_{0};
284 std::uint32_t crc32_{0};
285};
286
287} // namespace serial
288} // namespace jaiabot
289
290#endif
SerialThreadCRC32(const goby::middleware::protobuf::SerialConfig &config, int index=-1)
Definition crc32.h:11
uint32_t calculate_crc32(const void *buf, size_t len, uint32_t initial=0)
Definition crc32.h:43
constexpr int SERIAL_MAGIC_BYTES
static const std::string decode_frame(const std::string &frame_data)
constexpr auto CRC_SIZE
constexpr int SIZE_BYTES
constexpr const char * SERIAL_MAGIC
constexpr int BITS_IN_BYTE
constexpr auto SERIAL_MAX_SIZE
static const std::shared_ptr< goby::middleware::protobuf::IOData > encode_frame(const std::string &data)
uint32_t calculate_crc32(const void *buf, size_t len, uint32_t initial=0)
Definition crc32.h:43