VSM C++ SDK
Vehicle Specific Modules SDK
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
mavlink_decoder.h
Go to the documentation of this file.
1 // Copyright (c) 2018, Smart Projects Holdings Ltd
2 // All rights reserved.
3 // See LICENSE file for license details.
4 
10 #include <ugcs/vsm/callback.h>
11 #include <ugcs/vsm/mavlink.h>
12 
13 #include <unordered_map>
14 
15 #ifndef _UGCS_VSM_MAVLINK_DECODER_H_
16 #define _UGCS_VSM_MAVLINK_DECODER_H_
17 
18 namespace ugcs {
19 namespace vsm {
20 
23 private:
25  enum class State {
27  STX,
29  VER1,
31  VER2
32  };
33 
34 public:
42  uint8_t, uint8_t, uint32_t> Handler;
43 
46 
49  Make_decoder_handler,
50  (Io_buffer::Ptr, mavlink::MESSAGE_ID_TYPE, uint8_t, uint8_t, uint32_t),
51  (nullptr, mavlink::MESSAGE_ID::DEBUG_VALUE, mavlink::SYSTEM_ID_NONE, 0, 0))
52 
53 
54  DEFINE_CALLBACK_BUILDER(Make_raw_data_handler, (Io_buffer::Ptr), (nullptr))
55 
57  struct Stats {
59  uint64_t handled = 0;
61  uint64_t no_handler = 0;
63  uint64_t bad_checksum = 0;
65  uint64_t bad_length = 0;
69  uint64_t unknown_id = 0;
71  uint64_t bytes_received = 0;
74  uint64_t stx_syncs = 0;
75  };
76 
77 
80  packet_buf(ugcs::vsm::Io_buffer::Create())
81  {
82  }
83 
85  Mavlink_decoder(const Mavlink_decoder&) = delete;
86 
88  void
90  {
91  handler = Handler();
92  data_handler = Raw_data_handler();
93  }
94 
98  void
100  {
101  this->handler = handler;
102  }
103 
104  void
105  Register_raw_data_handler(Raw_data_handler handler)
106  {
107  data_handler = handler;
108  }
109 
111  void
113  {
114  // LOG_DBG("read: %s", buffer->Get_hex().c_str());
115  if (data_handler) {
116  data_handler(buffer);
117  }
118  packet_buf = packet_buf->Concatenate(buffer);
119  size_t packet_len;
120  const uint8_t* data;
121  next_read_len = 0;
122 
123  while (true) {
124  size_t buffer_len = packet_buf->Get_length();
125  stats[mavlink::SYSTEM_ID_ANY].bytes_received += buffer_len;
126  if (state == State::STX) {
127  size_t len_skipped = 0;
128  if (buffer_len < mavlink::MAVLINK_1_MIN_FRAME_LEN) {
129  // need at least minimum frame length of data.
130  next_read_len = mavlink::MAVLINK_1_MIN_FRAME_LEN - buffer_len;
131  break;
132  }
133  // look for signature in received data.
134  data = static_cast<const uint8_t*>(packet_buf->Get_data());
135  for (; len_skipped < buffer_len; len_skipped++, data++) {
136  if (*data == mavlink::START_SIGN) {
137  // found preamble. Start receiving payload.
138  state = State::VER1;
139  stats[mavlink::SYSTEM_ID_ANY].stx_syncs++;
140  // slice off the preamble.
141  packet_buf = packet_buf->Slice(1);
142  break;
143  }
144  if (*data == mavlink::START_SIGN2) {
145  // found preamble. Start receiving payload.
146  state = State::VER2;
147  stats[mavlink::SYSTEM_ID_ANY].stx_syncs++;
148  // slice off the preamble.
149  packet_buf = packet_buf->Slice(1);
150  break;
151  }
152  }
153  if (len_skipped) {
154  // slice off the skipped bytes.
155  packet_buf = packet_buf->Slice(len_skipped);
156  }
157  }
158  if (state == State::VER1 || state == State::VER2) {
159  size_t wrapper_len; // non-payload data len excluding signature.
160  if (state == State::VER1) {
161  wrapper_len = mavlink::MAVLINK_1_HEADER_LEN - 1 + 2;
162  } else {
163  wrapper_len = mavlink::MAVLINK_2_HEADER_LEN - 1 + 2;
164  }
165  buffer_len = packet_buf->Get_length();
166  if (buffer_len == 0) {
167  // need at least the minimum packet len. Initiate next read.
168  next_read_len = wrapper_len;
169  break;
170  }
171  data = static_cast<const uint8_t*>(packet_buf->Get_data());
172  packet_len = wrapper_len + static_cast<size_t>(*data);
173  if (packet_len > buffer_len) {
174  // need the whole packet. Initiate next read.
175  next_read_len = packet_len - buffer_len;
176  break;
177  }
178  if (Decode_packet(packet_buf)) {
179  // decoder suceeded. Slice off the decoded packet.
180  packet_buf = packet_buf->Slice(packet_len);
181  }
182  // if decoder failed, we restart the search for
183  // next preamble in existing data otherwise
184  // continue with next byte after the decoded packet.
185  state = State::STX;
186  }
187  }
188  }
189 
193  void
194  Reset(bool reset_stats = true)
195  {
196  state = State::STX;
197  packet_buf = ugcs::vsm::Io_buffer::Create();
198  if (reset_stats) {
199  stats.clear();
200  }
201  }
202 
207  size_t
209  {
210  return next_read_len;
211  }
212 
218  const Mavlink_decoder::Stats &
219  Get_stats(int system_id)
220  {
221  return stats[system_id];
222  }
223 
225  const Mavlink_decoder::Stats &
227  {
228  return stats[mavlink::SYSTEM_ID_ANY];
229  }
230 
231 private:
232  bool
233  Decode_packet(Io_buffer::Ptr buffer)
234  {
235  auto data = static_cast<const uint8_t*>(buffer->Get_data());
236  uint16_t payload_len = data[0];
237  uint8_t system_id;
238  uint8_t component_id;
239  uint8_t seq;
240  uint8_t header_len;
242 
243  if (state == State::VER2) {
244  seq = data[3];
245  system_id = data[4];
246  component_id = data[5];
247  msg_id =
248  static_cast<int>(data[6]) +
249  (static_cast<int>(data[7]) << 8) +
250  (static_cast<int>(data[8]) << 16);
251  header_len = mavlink::MAVLINK_2_HEADER_LEN - 1;
252  } else {
253  seq = data[1];
254  system_id = data[2];
255  component_id = data[3];
256  msg_id = data[4];
257  header_len = mavlink::MAVLINK_1_HEADER_LEN - 1;
258  }
259 
260  mavlink::Checksum sum(data, header_len);
261 
262  mavlink::Extra_byte_length_pair crc_byte_len_pair;
263  // Try all extensions.
264  if ( !sum.Get_extra_byte_length_pair(msg_id, crc_byte_len_pair, mavlink::Extension::Get())
265  && !sum.Get_extra_byte_length_pair(msg_id, crc_byte_len_pair, mavlink::apm::Extension::Get())
266  && !sum.Get_extra_byte_length_pair(msg_id, crc_byte_len_pair, mavlink::sph::Extension::Get())) {
267  stats[mavlink::SYSTEM_ID_ANY].unknown_id++;
268  // LOG_DEBUG("Unknown Mavlink message id: %d (%X)", msg_id, msg_id);
269  return false;
270  }
271 
272  sum.Accumulate(data + header_len, payload_len);
273 
274  uint16_t sum_calc = sum.Accumulate(crc_byte_len_pair.first);
275  /* Convert checksum in Mavlink byte order to a host byte order
276  * compatible type. */
277  auto sum_recv = reinterpret_cast<const mavlink::Uint16*>(data + header_len + payload_len);
278 
279  bool cksum_ok = sum_calc == *sum_recv;
280  bool length_ok = crc_byte_len_pair.second == payload_len;
281 
282  if (cksum_ok && (length_ok || state == State::VER2)) {
283  /*
284  * Fully valid packet received.
285  */
286  if (handler) {
287  handler(Io_buffer::Create(*buffer, header_len, payload_len), msg_id, system_id, component_id, seq);
288  stats[system_id].handled++;
289  stats[mavlink::SYSTEM_ID_ANY].handled++;
290  } else {
291  stats[system_id].no_handler++;
292  stats[mavlink::SYSTEM_ID_ANY].no_handler++;
293  LOG_DEBUG("Mavlink message %d handler not registered.", msg_id);
294  }
295  return true;
296  } else {
297  if (cksum_ok) {
298  stats[system_id].bad_length++;
299  stats[mavlink::SYSTEM_ID_ANY].bad_length++;
300  LOG_DEBUG("Mavlink payload length mismatch, recv=%d wanted=%d.",
301  payload_len, crc_byte_len_pair.second);
302  } else {
303  stats[mavlink::SYSTEM_ID_ANY].bad_checksum++;
304  }
305  return false;
306  }
307  }
308 
309 
311  State state = State::STX;
312 
314  Handler handler;
315 
317  Raw_data_handler data_handler;
318 
320  std::unordered_map<int, Stats> stats;
321 
323  ugcs::vsm::Io_buffer::Ptr packet_buf;
324 
325  size_t next_read_len = mavlink::MAVLINK_1_MIN_FRAME_LEN;
326 };
327 
328 } /* namespace vsm */
329 } /* namespace ugcs */
330 
331 #endif /* _UGCS_VSM_MAVLINK_DECODER_H_ */
#define LOG_DEBUG(msg,...)
Write debug message to the application log.
Definition: log.h:323
#define DEFINE_CALLBACK_BUILDER(__name, __types, __values)
Define callback builder function.
Definition: callback.h:42
static Ptr Create(Args &&...args)
Create an instance.
Definition: io_buffer.h:34
Generic callback which can be used to define and create an instance of an abstract callable operation...
Helper class for proxying callback invocation.
Definition: callback.h:694
T Get() const
Get the value of underlying type.
Definition: endian.h:324
Generic I/O buffer.
Definition: io_buffer.h:33
std::shared_ptr< Io_buffer > Ptr
Pointer type.
Definition: io_buffer.h:34