JaiaBot 3.0.0
JaiaBot micro-AUV software
Loading...
Searching...
No Matches
mcu.h
Go to the documentation of this file.
1#ifndef JAIABOT_SERIAL_MCU_H
2#define JAIABOT_SERIAL_MCU_H
3
4// helper functions for encoding/decoding messages from microcontrollers
5
6#include <goby/middleware/protobuf/io.pb.h>
7
8#include "jaiabot/crc/crc32.h"
9
11{
12
13constexpr int bits_in_byte = 8;
14constexpr int bytes_in_crc32 = 4;
15
16inline std::shared_ptr<goby::middleware::protobuf::IOData>
17encode_for_mcu(const google::protobuf::Message& pb_msg)
18{
19 auto io_msg = std::make_shared<goby::middleware::protobuf::IOData>();
20 std::string* encoded = io_msg->mutable_data();
21 pb_msg.SerializeToString(encoded);
22
23 std::uint32_t crc32_value = crc::calculate_crc32(encoded->data(), encoded->size());
24
25 for (int i = bytes_in_crc32 - 1; i >= 0; --i)
26 {
27 encoded->push_back((crc32_value >> (i * bits_in_byte)) & 0xFF);
28 }
29 return io_msg;
30}
31
32template <typename ProtobufMessage>
33ProtobufMessage decode_from_mcu(const goby::middleware::protobuf::IOData& io_msg)
34{
35 const auto& encoded = io_msg.data();
36
37 if (encoded.size() < bytes_in_crc32)
38 throw(std::runtime_error("Message is too small"));
39
40 std::uint32_t computed_crc =
41 crc::calculate_crc32(encoded.data(), encoded.size() - bytes_in_crc32);
42 std::uint32_t provided_crc = 0;
43
44 std::size_t i = 0;
45 for (auto it = encoded.rbegin(), end = encoded.rbegin() + bytes_in_crc32; it != end; ++it, ++i)
46 // cast is required as char is signed on some platforms, which would sign-extend
47 // any CRC byte >= 0x80 and corrupt the comparison
48 provided_crc |= static_cast<std::uint32_t>(static_cast<std::uint8_t>(*it))
49 << (i * bits_in_byte);
50
51 if (computed_crc != provided_crc)
52 {
53 throw(std::runtime_error("Computed CRC (" + std::to_string(computed_crc) +
54 ") does not equal CRC on message (" +
55 std::to_string(provided_crc) + ")"));
56 }
57
58 ProtobufMessage pb_msg;
59 pb_msg.ParseFromArray(encoded.data(), encoded.size() - bytes_in_crc32);
60 return pb_msg;
61}
62
63} // namespace jaiabot::serial
64
65#endif
uint32_t calculate_crc32(const void *buf, size_t len, uint32_t initial=0)
Definition crc32.h:43
std::shared_ptr< goby::middleware::protobuf::IOData > encode_for_mcu(const google::protobuf::Message &pb_msg)
Definition mcu.h:17
ProtobufMessage decode_from_mcu(const goby::middleware::protobuf::IOData &io_msg)
Definition mcu.h:33
constexpr int bits_in_byte
Definition mcu.h:13
constexpr int bytes_in_crc32
Definition mcu.h:14