Goby3  3.1.5a
2024.05.23
mavlink.h
Go to the documentation of this file.
1 // Copyright 2019-2021:
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_MARSHALLING_MAVLINK_H
25 #define GOBY_MIDDLEWARE_MARSHALLING_MAVLINK_H
26 
27 #include <array> // for array<>::iterator
28 #include <cstdint> // for uint8_t, uint32_t
29 #include <memory> // for shared_ptr, make_sh...
30 #include <mutex> // for mutex, lock_guard
31 #include <ostream> // for basic_ostream, endl
32 #include <string> // for operator<<, string
33 #include <tuple> // for tuple, make_tuple
34 #include <type_traits> // for enable_if, is_base_of
35 #include <unordered_map> // for unordered_map, oper...
36 #include <utility> // for make_pair, pair
37 #include <vector> // for vector
38 
39 #include <mavlink/v2.0/common/common.hpp>
40 
41 #include "goby/util/debug_logger/flex_ostream.h" // for operator<<, FlexOst...
42 
43 #include "interface.h" // for MarshallingScheme
44 
45 namespace goby
46 {
47 namespace middleware
48 {
53 {
57  template <std::size_t Size>
58  static void register_dialect_entries(std::array<mavlink::mavlink_msg_entry_t, Size> entries)
59  {
60  std::lock_guard<std::mutex> lock(mavlink_registry_mutex_);
61  for (const auto& entry : entries) entries_.insert(std::make_pair(entry.msgid, entry));
62  }
63 
68  static const mavlink::mavlink_msg_entry_t* get_msg_entry(uint32_t msgid)
69  {
70  if (entries_.empty())
72 
73  std::lock_guard<std::mutex> lock(mavlink_registry_mutex_);
74  auto it = entries_.find(msgid);
75  if (it != entries_.end())
76  return &it->second;
77  else
78  return nullptr;
79  }
80 
81  static void register_default_dialects();
82 
83  private:
84  static std::unordered_map<uint32_t, mavlink::mavlink_msg_entry_t> entries_;
85  static std::mutex mavlink_registry_mutex_;
86 };
87 
89 template <> struct SerializerParserHelper<mavlink::mavlink_message_t, MarshallingScheme::MAVLINK>
90 {
91  static std::vector<char> serialize(const mavlink::mavlink_message_t& msg)
92  {
93  std::array<uint8_t, MAVLINK_MAX_PACKET_LEN> buffer;
94  auto length = mavlink::mavlink_msg_to_send_buffer(&buffer[0], &msg);
95  return std::vector<char>(buffer.begin(), buffer.begin() + length);
96  }
97 
98  static std::string type_name(const mavlink::mavlink_message_t& msg)
99  {
100  return std::to_string(msg.msgid);
101  }
102 
103  template <typename CharIterator>
104  static std::shared_ptr<mavlink::mavlink_message_t>
105  parse(CharIterator bytes_begin, CharIterator bytes_end, CharIterator& actual_end,
106  const std::string& type)
107  {
108  CharIterator c = bytes_begin;
109  auto msg = std::make_shared<mavlink::mavlink_message_t>();
110  mavlink::mavlink_message_t msg_buffer{};
111  mavlink::mavlink_status_t status{}, status_buffer{};
112  while (c != bytes_end)
113  {
114  auto res = mavlink::mavlink_frame_char_buffer(&msg_buffer, &status_buffer, *c,
115  msg.get(), &status);
116 
117  switch (res)
118  {
119  case mavlink::MAVLINK_FRAMING_INCOMPLETE: ++c; break;
120 
121  case mavlink::MAVLINK_FRAMING_OK:
122  {
123  actual_end = c;
124  return msg;
125  }
126 
127  case mavlink::MAVLINK_FRAMING_BAD_CRC:
128  if (!MAVLinkRegistry::get_msg_entry(msg->msgid))
129  {
130  goby::glog.is_debug2() &&
131  goby::glog << "MAVLink msg type: " << type
132  << " is unknown, so unable to validate CRC" << std::endl;
133  }
134  else
135  {
136  goby::glog.is_warn() &&
137  goby::glog << "BAD CRC decoding MAVLink type: " << type << std::endl;
138  }
139  goto fail;
140  break;
141  case mavlink::MAVLINK_FRAMING_BAD_SIGNATURE:
142  goby::glog.is_warn() &&
143  goby::glog << "BAD SIGNATURE decoding MAVLink type: " << type << std::endl;
144  goto fail;
145 
146  break;
147  default:
149  << "Unknown value " << res
150  << " returned while decoding MAVLink type: " << type
151  << std::endl;
152  goto fail;
153 
154  break;
155  }
156  }
157  fail:
158  actual_end = bytes_end;
159  return msg;
160  }
161 };
162 
164 {
165  enum
166  {
170  };
171 };
172 
174 template <typename DataType, typename Integer>
175 struct SerializerParserHelper<std::tuple<Integer, Integer, DataType>, MarshallingScheme::MAVLINK>
176 {
177  static std::vector<char>
178  serialize(const std::tuple<Integer, Integer, DataType>& packet_with_metadata)
179  {
180  mavlink::mavlink_message_t msg{};
181  mavlink::MsgMap map(msg);
182  const DataType& packet = std::get<MAVLinkTupleIndices::PACKET_INDEX>(packet_with_metadata);
183  packet.serialize(map);
184  mavlink::mavlink_finalize_message(
185  &msg, std::get<MAVLinkTupleIndices::SYSTEM_ID_INDEX>(packet_with_metadata),
186  std::get<MAVLinkTupleIndices::COMPONENT_ID_INDEX>(packet_with_metadata),
187  packet.MIN_LENGTH, packet.LENGTH, packet.CRC_EXTRA);
188  return SerializerParserHelper<mavlink::mavlink_message_t,
190  }
191 
192  // use numeric type name since that's all we have with mavlink_message_t alone
193  static std::string type_name() { return std::to_string(DataType::MSG_ID); }
194  static std::string type_name(const std::tuple<Integer, Integer, DataType>& /*d*/)
195  {
196  return type_name();
197  }
198 
199  template <typename CharIterator>
200  static std::shared_ptr<std::tuple<Integer, Integer, DataType>>
201  parse(CharIterator bytes_begin, CharIterator bytes_end, CharIterator& actual_end,
202  const std::string& /*type*/ = type_name())
203  {
204  auto msg =
206  bytes_begin, bytes_end, actual_end, type_name());
207  auto packet_with_metadata = std::make_shared<std::tuple<Integer, Integer, DataType>>();
208  DataType* packet = &std::get<MAVLinkTupleIndices::PACKET_INDEX>(*packet_with_metadata);
209  mavlink::MsgMap map(msg.get());
210  packet->deserialize(map);
211  std::get<MAVLinkTupleIndices::SYSTEM_ID_INDEX>(*packet_with_metadata) = msg->sysid;
212  std::get<MAVLinkTupleIndices::COMPONENT_ID_INDEX>(*packet_with_metadata) = msg->compid;
213  return packet_with_metadata;
214  }
215 };
216 
218 template <typename DataType> struct SerializerParserHelper<DataType, MarshallingScheme::MAVLINK>
219 {
220  static std::vector<char> serialize(const DataType& packet, int sysid = 1, int compid = 1)
221  {
223  serialize(std::make_tuple(sysid, compid, packet));
224  }
225 
226  static std::string type_name()
227  {
230  }
231  static std::string type_name(const DataType& /*d*/) { return type_name(); }
232 
233  template <typename CharIterator>
234  static std::shared_ptr<DataType> parse(CharIterator bytes_begin, CharIterator bytes_end,
235  CharIterator& actual_end,
236  const std::string& /*type*/ = type_name())
237  {
238  auto packet_with_metadata =
240  MarshallingScheme::MAVLINK>::parse(bytes_begin, bytes_end,
241  actual_end);
242  return std::make_shared<DataType>(
243  std::get<MAVLinkTupleIndices::PACKET_INDEX>(*packet_with_metadata));
244  }
245 };
246 
248 template <typename Tuple,
250  typename std::enable_if<std::is_base_of<mavlink::Message, T>::value>::type* = nullptr>
251 constexpr int scheme()
252 {
254 }
255 
257 template <typename T, typename std::enable_if<
258  std::is_base_of<mavlink::Message, T>::value ||
259  std::is_same<mavlink::mavlink_message_t, T>::value>::type* = nullptr>
260 constexpr int scheme()
261 {
263 }
264 
265 } // namespace middleware
266 } // namespace goby
267 
268 #endif
goby::middleware::MarshallingScheme
Enumeration and helper functions for marshalling scheme identification.
Definition: interface.h:45
goby::middleware::SerializerParserHelper::parse
static std::shared_ptr< DataType > parse(CharIterator bytes_begin, CharIterator bytes_end, CharIterator &actual_end, const std::string &type=type_name())
Given a beginning and end iterator to bytes, parse the data and return it.
Definition: interface.h:129
goby::middleware::MAVLinkTupleIndices::SYSTEM_ID_INDEX
@ SYSTEM_ID_INDEX
Definition: mavlink.h:167
interface.h
goby
The global namespace for the Goby project.
Definition: acomms_constants.h:33
goby::middleware::SerializerParserHelper
Class for parsing and serializing a given marshalling scheme. Must be specialized for a particular sc...
Definition: interface.h:97
goby::util::FlexOstream::is_warn
bool is_warn()
Definition: flex_ostream.h:82
goby::util::logger::mutex
std::recursive_mutex mutex
goby::middleware::SerializerParserHelper::serialize
static std::vector< char > serialize(const DataType &)
Given data, produce a vector of bytes.
Definition: interface.h:100
goby::middleware::MAVLinkTupleIndices
Definition: mavlink.h:163
goby::middleware::SerializerParserHelper< std::tuple< Integer, Integer, DataType >, MarshallingScheme::MAVLINK >::type_name
static std::string type_name()
Definition: mavlink.h:193
goby::util::FlexOstream::is_debug2
bool is_debug2()
Definition: flex_ostream.h:85
goby::middleware::SerializerParserHelper< std::tuple< Integer, Integer, DataType >, MarshallingScheme::MAVLINK >::type_name
static std::string type_name(const std::tuple< Integer, Integer, DataType > &)
Definition: mavlink.h:194
goby::util::logger_lock::lock
@ lock
Definition: flex_ostreambuf.h:62
goby::middleware::MAVLinkRegistry::get_msg_entry
static const mavlink::mavlink_msg_entry_t * get_msg_entry(uint32_t msgid)
Retrieve a entry given a message id.
Definition: mavlink.h:68
goby::middleware::acomms::groups::status
constexpr Group status
Definition: groups.h:43
goby::middleware::SerializerParserHelper::type_name
static std::string type_name()
The marshalling scheme specific string name for this type.
Definition: interface.h:107
goby::middleware::SerializerParserHelper< std::tuple< Integer, Integer, DataType >, MarshallingScheme::MAVLINK >::serialize
static std::vector< char > serialize(const std::tuple< Integer, Integer, DataType > &packet_with_metadata)
Definition: mavlink.h:178
goby::middleware::MAVLinkRegistry
A registry of mavlink types used for decoding.
Definition: mavlink.h:52
to_string
NLOHMANN_BASIC_JSON_TPL_DECLARATION std::string to_string(const NLOHMANN_BASIC_JSON_TPL &j)
user-defined to_string function for JSON values
Definition: json.hpp:24301
jwt::json::type
type
Generic JSON types used in JWTs.
Definition: jwt.h:2071
flex_ostream.h
goby::msg
extern ::google::protobuf::internal::ExtensionIdentifier< ::google::protobuf::MessageOptions, ::google::protobuf::internal::MessageTypeTraits< ::goby::GobyMessageOptions >, 11, false > msg
Definition: option_extensions.pb.h:1327
goby::middleware::MAVLinkTupleIndices::PACKET_INDEX
@ PACKET_INDEX
Definition: mavlink.h:169
goby::middleware::SerializerParserHelper< std::tuple< Integer, Integer, DataType >, MarshallingScheme::MAVLINK >::parse
static std::shared_ptr< std::tuple< Integer, Integer, DataType > > parse(CharIterator bytes_begin, CharIterator bytes_end, CharIterator &actual_end, const std::string &=type_name())
Definition: mavlink.h:201
goby::middleware::MAVLinkRegistry::register_dialect_entries
static void register_dialect_entries(std::array< mavlink::mavlink_msg_entry_t, Size > entries)
Register a new Mavlink dialect.
Definition: mavlink.h:58
goby::middleware::MAVLinkTupleIndices::COMPONENT_ID_INDEX
@ COMPONENT_ID_INDEX
Definition: mavlink.h:168
goby::middleware::scheme
constexpr int scheme()
Placeholder to provide an interface for the scheme() function family.
Definition: cstr.h:65
goby::glog
util::FlexOstream glog
Access the Goby logger through this object.
goby::middleware::MarshallingScheme::MAVLINK
@ MAVLINK
Definition: interface.h:58
goby::middleware::MAVLinkRegistry::register_default_dialects
static void register_default_dialects()