VSM C++ SDK
Vehicle Specific Modules SDK
mavlink_decoder.h
Go to the documentation of this file.
1 // Copyright (c) 2014, 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 #ifndef _MAVLINK_DECODER_H_
14 #define _MAVLINK_DECODER_H_
15 
16 namespace ugcs {
17 namespace vsm {
18 
20 template<typename Mavlink_kind>
22 
23 private:
24 
26  enum class State {
28  STX,
30  HEADER,
32  PAYLOAD,
34  CHECKSUM
35  };
36 
37 public:
38 
41 
49  typename Mavlink_kind::System_id, uint8_t, uint32_t> Handler;
50 
53 
56  Make_decoder_handler,
57  (Io_buffer::Ptr, mavlink::MESSAGE_ID_TYPE, typename Mavlink_kind::System_id, uint8_t, uint32_t),
58  (nullptr, mavlink::MESSAGE_ID::DEBUG_VALUE, mavlink::SYSTEM_ID_NONE, 0, 0))
59 
60 
61  DEFINE_CALLBACK_BUILDER(Make_raw_data_handler, (Io_buffer::Ptr), (nullptr))
62 
64  struct Stats {
66  uint64_t handled;
68  uint64_t no_handler;
70  uint64_t bad_checksum;
72  uint64_t bad_length;
76  uint64_t unknown_id;
78  uint64_t bytes_received;
81  uint64_t stx_syncs;
82  };
83 
84 
87  stats()
88  {}
89 
91  Mavlink_decoder(const Mavlink_decoder&) = delete;
92 
94  void
96  {
97  handler = Handler();
98  data_handler = Raw_data_handler();
99  }
100 
104  void
105  Register_handler(Handler handler)
106  {
107  this->handler = handler;
108  }
109 
110  void
111  Register_raw_data_handler(Raw_data_handler handler)
112  {
113  data_handler = handler;
114  }
115 
117  void
118  Decode(Io_buffer::Ptr buffer)
119  {
120  if (data_handler) {
121  data_handler(buffer);
122  }
123  Decode_buffer(buffer);
124 
125  /* Do we have accumulated segments which could not be processed by a
126  * full turn of state machine, i.e. back to STX?
127  */
128  while (segments.size() && state == State::STX) {
129  auto retry_segs = std::move(segments);
130  do {
131  Decode_buffer(retry_segs.front());
132  retry_segs.pop_front();
133  /* Loop while in the middle of packet decoding. */
134  } while (retry_segs.size() && state != State::STX);
135  /* Join newly accumulated and remaining segments. */
136  segments.splice(segments.end(), retry_segs);
137  }
138  }
139 
143  void
144  Reset(bool reset_stats = true)
145  {
146  state = State::STX;
147  payload = nullptr;
148  header.clear();
149  checksum.clear();
150  if (reset_stats) {
151  stats = Stats();
152  }
153  }
154 
159  size_t
161  {
162  const Header* header_ptr;
163 
164  switch (state) {
165  /*
166  * In STX and HEADER states we don't want to read any payload data,
167  * because we want to receive the whole payload at once together with
168  * checksum to avoid memory copy.
169  */
170  case State::STX: return sizeof(Header);
171  case State::HEADER: return sizeof(Header) - header.size();
172  case State::PAYLOAD:
173  header_ptr =
174  reinterpret_cast<const Header*>(header.data());
175  return header_ptr->payload_len - payload->Get_length() + sizeof(uint16_t);
176  case State::CHECKSUM: return sizeof(uint16_t) - checksum.size();
177  default:
178  ASSERT(false);
179  VSM_EXCEPTION(Internal_error_exception, "Unexpected state %d",
180  static_cast<int>(state));
181  }
182  }
183 
185  const Mavlink_decoder::Stats &
186  Get_stats() const
187  {
188  return stats;
189  }
190 
191 private:
192 
194  void
195  Decode_buffer(Io_buffer::Ptr buffer)
196  {
197  stats.bytes_received += buffer->Get_length();
198 
199  while (buffer->Get_length()) {
200  switch (state) {
201  case State::STX:
202  buffer = Decode_stx(buffer);
203  break;
204  case State::HEADER:
205  buffer = Decode_header(buffer);
206  break;
207  case State::PAYLOAD:
208  buffer = Decode_payload(buffer);
209  break;
210  case State::CHECKSUM:
211  buffer = Decode_checksum(buffer);
212  break;
213  }
214  }
215  }
216 
218  Io_buffer::Ptr
219  Decode_stx(Io_buffer::Ptr buffer)
220  {
221  const uint8_t* data = static_cast<const uint8_t*>(buffer->Get_data());
222  size_t full_len = buffer->Get_length();
223  size_t len = full_len;
224 
225  for ( ; len ; len--, data++) {
226  if (*data == mavlink::START_SIGN) {
227  stats.stx_syncs++;
228  state = State::HEADER;
229  header.clear();
230  break;
231  }
232  }
233 
234  return buffer->Slice(full_len - len);
235  }
236 
238  Io_buffer::Ptr
239  Decode_header(Io_buffer::Ptr buffer)
240  {
241  const uint8_t* data = static_cast<const uint8_t*>(buffer->Get_data());
242  size_t len = buffer->Get_length();
243 
244  for ( ; len && header.size() < sizeof(Header) ; len--, data++) {
245  header.push_back(*data);
246  }
247 
248  if (header.size() == sizeof(Header)) {
249  state = State::PAYLOAD;
250  /* Strip STX byte. */
251  segments.push_back(Io_buffer::Create(&header[1], header.size() - 1));
252  payload = Io_buffer::Create();
253  }
254 
255  return buffer->Slice(buffer->Get_length() - len);
256  }
257 
259  Io_buffer::Ptr
260  Decode_payload(Io_buffer::Ptr buffer)
261  {
262  Header* header_ptr = reinterpret_cast<Header*>(header.data());
263 
264  size_t to_read = std::min(header_ptr->payload_len - payload->Get_length(),
265  buffer->Get_length());
266 
267  payload = payload->Concatenate(buffer->Slice(0, to_read));
268 
269  if (payload->Get_length() == header_ptr->payload_len) {
270  state = State::CHECKSUM;
271  segments.push_back(payload);
272  checksum.clear();
273  }
274 
275  return buffer->Slice(to_read);
276  }
277 
279  Io_buffer::Ptr
280  Decode_checksum(Io_buffer::Ptr buffer)
281  {
282  const uint8_t* data = static_cast<const uint8_t*>(buffer->Get_data());
283  size_t len = buffer->Get_length();
284 
285  for ( ; len && checksum.size() < sizeof(uint16_t) ; len--, data++) {
286  checksum.push_back(*data);
287  }
288 
289  if (checksum.size() == sizeof(uint16_t)) {
290  /* Got checksum, state machine should be restarted. */
291  state = State::STX;
292  segments.push_back(Io_buffer::Create(&checksum[0], checksum.size()));
293  Header* header_ptr = reinterpret_cast<Header*>(header.data());
294 
295  mavlink::Checksum sum(&header[1], header.size() - 1);
296  sum.Accumulate(payload);
297 
298  mavlink::Extra_byte_length_pair crc_byte_len_pair;
299  // Try all extensions.
300  if ( !sum.Get_extra_byte_length_pair(header_ptr->message_id, crc_byte_len_pair, mavlink::Extension::Get())
301  && !sum.Get_extra_byte_length_pair(header_ptr->message_id, crc_byte_len_pair, mavlink::apm::Extension::Get())
302  && !sum.Get_extra_byte_length_pair(header_ptr->message_id, crc_byte_len_pair, mavlink::sph::Extension::Get())) {
303  stats.unknown_id++;
304  LOG_DEBUG("Unknown Mavlink message id: %d", header_ptr->message_id.Get());
305  /* Save remaining bytes. */
306  segments.push_back(buffer->Slice(buffer->Get_length() - len));
307  /* And terminate the decoding of current buffer. */
308  return Io_buffer::Create();
309  }
310 
311  uint16_t sum_calc = sum.Accumulate(crc_byte_len_pair.first);
312  /* Convert checksum in Mavlink byte order to a host byte order
313  * compatible type. */
314  mavlink::Uint16* sum_recv = reinterpret_cast<mavlink::Uint16*>(checksum.data());
315 
316  bool cksum_ok = sum_calc == *sum_recv;
317  bool length_ok = crc_byte_len_pair.second == header_ptr->payload_len;
318 
319  if (cksum_ok && length_ok) {
320  /*
321  * Fully valid packet received, not interested in its segments
322  * anymore.
323  */
324  segments.clear();
325  mavlink::MESSAGE_ID_TYPE message_id = header_ptr->message_id.Get();
326  if (handler) {
327  handler(payload, message_id, header_ptr->system_id, header_ptr->component_id, header_ptr->seq);
328  stats.handled++;
329  } else {
330  stats.no_handler++;
331  LOG_DEBUG("Mavlink message %d handler not registered.", message_id);
332  }
333  } else {
334  if (!cksum_ok) {
335  stats.bad_checksum++;
336  LOG_DEBUG("Mavlink checksum mismatch, recv=%x calc=%x.",
337  sum_recv->Get(), sum_calc);
338  } else {
339  stats.bad_length++;
340  LOG_DEBUG("Mavlink payload length mismatch, recv=%d wanted=%d.",
341  header_ptr->payload_len.Get(), crc_byte_len_pair.second);
342  }
343  /* Save remaining bytes. */
344  segments.push_back(buffer->Slice(buffer->Get_length() - len));
345  /* And terminate the decoding of current buffer. */
346  return Io_buffer::Create();
347  }
348  }
349 
350  return buffer->Slice(buffer->Get_length() - len);
351  }
352 
353 
355  State state = State::STX;
356 
358  std::vector<uint8_t> header;
359 
361  std::vector<uint8_t> checksum;
362 
364  Io_buffer::Ptr payload;
365 
367  Handler handler;
368 
370  Raw_data_handler data_handler;
371 
373  Stats stats;
374 
379  std::list<Io_buffer::Ptr> segments;
380 
381 };
382 
383 } /* namespace vsm */
384 } /* namespace ugcs */
385 
386 #endif /* _MAVLINK_DECODER_H_ */
UGCS root namespace.
Definition: android-linux/ugcs/vsm/platform_sockets.h:27
#define LOG_DEBUG(msg,...)
Write debug message to the application log.
Definition: log.h:326
Exception class with one parameter.
Definition: exception.h:88
#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...
#define ASSERT(x)
No action in release.
Definition: debug.h:68
Helper class for proxying callback invocation.
Definition: callback.h:691
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
#define VSM_EXCEPTION(__exc_class, __msg,...)
Throw VSM exception instance.
Definition: exception.h:170