VSM C++ SDK
Vehicle Specific Modules SDK
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
mavlink.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 
11 #ifndef _UGCS_VSM_MAVLINK_H_
12 #define _UGCS_VSM_MAVLINK_H_
13 
14 #include <ugcs/vsm/endian.h>
15 #include <ugcs/vsm/io_buffer.h>
16 
17 #include <cstring>
18 #include <map>
19 #include <cmath>
20 #include <type_traits>
21 
22 namespace ugcs {
23 namespace vsm {
24 
26 namespace mavlink {
27 
28 enum {
30  VERSION = 3,
32  START_SIGN = 0xfe,
34  START_SIGN2 = 0xfd,
39 
42 };
43 
44 static constexpr size_t MAVLINK_1_HEADER_LEN = 6;
45 static constexpr size_t MAVLINK_1_MIN_FRAME_LEN = MAVLINK_1_HEADER_LEN + 2;
46 
47 static constexpr size_t MAVLINK_2_HEADER_LEN = 10;
48 static constexpr size_t MAVLINK_2_MIN_FRAME_LEN = MAVLINK_2_HEADER_LEN + 2;
49 
54  /* MAVLink standard types follow. */
55  CHAR,
56  INT8,
57  UINT8,
58  INT16,
59  UINT16,
60  INT32,
61  UINT32,
62  INT64,
63  UINT64,
64  FLOAT,
65  DOUBLE,
66  UINT8_VERSION,
67 
70 };
71 
73 template<class T, class = void>
75 };
76 
78 template<class T>
79 struct Field_default_value<T, typename std::enable_if<std::is_integral<T>::value>::type> {
81  static constexpr T value = std::numeric_limits<T>::max();
82 
84  static bool
85  Is_default(T val)
86  {
87  return val == value;
88  }
89 };
90 
92 template<class T>
93 struct Field_default_value<T, typename std::enable_if<std::is_floating_point<T>::value>::type> {
94  static constexpr T value = std::numeric_limits<T>::quiet_NaN();
95 
97  static bool
98  Is_default(T val)
99  {
100  return std::isnan(val);
101  }
102 };
110 template <typename T, Field_type_id id, int32_t initial_value = 0>
111 class Value {
112 public:
117  Value(T value = initial_value): value(value)
118  {}
119 
124  Value &
125  operator =(T value)
126  {
127  this->value = value;
128  return *this;
129  }
130 
135  operator T() const
136  {
137  return value;
138  }
139 
144  T
145  Get() const
146  {
147  return value;
148  }
149 
151  constexpr Field_type_id
153  {
154  return id;
155  }
156 
158  void
160  {
162  }
163 
165  bool
166  Is_reset() const
167  {
169  }
170 
171 private:
173  Le_value<T> value;
174 } __PACKED;
175 
176 // @{
179 typedef Value<int8_t, INT8> Int8;
187 typedef Value<float, FLOAT> Float;
190 // @}
191 
196 template <class TValue, size_t size>
197 class Value_array {
198 private:
200  TValue data[size];
201 public:
203  TValue &
204  operator[](size_t index)
205  {
206  if (index >= size) {
207  VSM_EXCEPTION(Invalid_param_exception, "Index out of range");
208  }
209  return data[index];
210  }
213  void
215  {
216  for (auto& elem : data) {
217  elem.Reset();
218  }
219  }
220 } __PACKED;
221 
225 template <size_t size>
226 class Value_array<Char, size> {
227 private:
229  Char data[size];
230 
231 public:
233  Char &
234  operator[](size_t index)
235  {
236  if (index >= size) {
237  VSM_EXCEPTION(Invalid_param_exception, "Index out of range");
238  }
239  return data[index];
240  }
241 
247  size_t
248  Get_length() const
249  {
250  size_t len = 0;
251  for (Char c : data) {
252  if (!c) {
253  return len;
254  }
255  len++;
256  }
257  return len;
258  }
259 
261  std::string
262  Get_string() const
263  {
264  return std::string(reinterpret_cast<const char *>(data), Get_length());
265  }
266 
268  void
270  {
271  for (auto& elem : data) {
272  elem = 0;
273  }
274  }
275 
276 #ifndef NO_DOXYGEN // Doxygen complains about such operator...
277 
278  operator std::string() const
279  {
280  return Get_string();
281  }
282 #endif
283 
285  bool
286  operator ==(const char *str)
287  {
288  return Get_string() == str;
289  }
290 
292  bool
293  operator !=(const char *str)
294  {
295  return Get_string() != str;
296  }
297 
304  Value_array &
305  operator =(const char *str)
306  {
307  size_t idx;
308  for (idx = 0; idx < size && *str; idx++, str++) {
309  data[idx] = *str;
310  }
311  if (idx < size) {
312  data[idx] = 0;
313  }
314  return *this;
315  }
316 
323  Value_array &
324  operator =(const std::string &str)
325  {
326  return *this = str.c_str();
327  }
328 } __PACKED;
329 
330 namespace internal {
331 
337  const char *name;
341  size_t array_size;
342 };
343 
344 } /* namespace internal */
345 
348 typedef std::pair<uint32_t, uint16_t> Extra_byte_length_pair;
349 
353 typedef uint32_t MESSAGE_ID_TYPE;
354 
355 /* Forward declaration for a type which will be fully defined in generated headers. */
356 enum MESSAGE_ID: MESSAGE_ID_TYPE;
357 
359 class Extension {
360 public:
361  Extension() {}
362 
364  virtual
366 
368  static const Extension &
369  Get()
370  {
371  return instance;
372  }
373 
375  virtual std::string
376  Get_name() const
377  {
378  return std::string();
379  }
380 
382  virtual const std::map<MESSAGE_ID_TYPE, Extra_byte_length_pair> *
384  {
385  return &crc_extra_bytes_length_map;
386  }
387 
388 private:
389  static const Extension instance;
393  static const std::map<MESSAGE_ID_TYPE, Extra_byte_length_pair> crc_extra_bytes_length_map;
394 };
395 
397 class Payload_base: public std::enable_shared_from_this<Payload_base> {
399 
400 public:
401  virtual
402  ~Payload_base()
403  {}
404 
406  virtual size_t
407  Get_size_v1() const = 0;
408 
410  virtual size_t
411  Get_size_v2() const = 0;
412 
415  Get_buffer() const;
416 
418  std::string
419  Dump() const;
420 
422  virtual const char *
423  Get_name() const = 0;
424 
426  virtual MESSAGE_ID_TYPE
427  Get_id() const = 0;
428 
430  virtual uint8_t
431  Get_extra_byte() const = 0;
432 
434  virtual void
435  Reset() = 0;
436 
437 protected:
439  virtual const void *
440  Get_data() const = 0;
441 
448  Get_fields() const = 0;
449 };
450 
451 typedef std::vector<Payload_base::Ptr> Payload_list;
452 
456 template <class TData, internal::Field_descriptor *fields, const char *msg_name,
457  MESSAGE_ID_TYPE msg_id, uint8_t extra_byte>
458 class Payload: public Payload_base {
460 
461 public:
463  Payload() = default;
464 
474  Payload(const void *buf, size_t size)
475  {
476  // The code below uses memcpy to packed structures to simplify parsing of mavlink messages.
477  // It is not valid if dest class has vtables or other magic.
478  // This assert protects us from overwriting payload instance with invalid data.
479  static_assert(std::is_trivially_copyable<TData>::value, "Payload is not trivially copyable.");
480  void* ptr = &data;
481  if (size > sizeof(data)) {
482  size = sizeof(data);
483  } else if (size < sizeof(data)) {
484  memset(ptr, 0, sizeof(data));
485  }
486  memcpy(ptr, buf, size);
487  }
488 
494  Payload(buffer->Get_data(), buffer->Get_length())
495  {}
496 
498  virtual size_t
499  Get_size_v1() const override
500  {
501  return data.Get_size_v1();
502  }
503 
505  virtual size_t
506  Get_size_v2() const override
507  {
508  return data.Get_size_v2();
509  }
510 
512  virtual const char *
513  Get_name() const override
514  {
515  return msg_name;
516  }
517 
519  virtual MESSAGE_ID_TYPE
520  Get_id() const override
521  {
522  return msg_id;
523  }
524 
526  virtual uint8_t
527  Get_extra_byte() const override
528  {
529  return extra_byte;
530  }
531 
533  virtual void
534  Reset() override
535  {
536  data.Reset();
537  }
538 
540  bool
541  operator ==(const Payload &rhs) const
542  {
543  return !memcmp(&data, &rhs.data, sizeof(data));
544  }
545 
547  bool
548  operator !=(const Payload &rhs) const
549  {
550  return memcmp(&data, &rhs.data, sizeof(data));
551  }
552 
554  TData *
556  {
557  return &data;
558  }
559 
561  const TData *
562  operator ->() const
563  {
564  return &data;
565  }
566 
568  TData &
570  {
571  return data;
572  }
573 
575  const TData &
576  operator *() const
577  {
578  return data;
579  }
580 
581 private:
583  TData data;
584 
586  virtual const void *
587  Get_data() const override
588  {
589  return &data;
590  }
591 
597  virtual internal::Field_descriptor *
598  Get_fields() const override
599  {
600  return fields;
601  }
602 };
603 
607 template <MESSAGE_ID_TYPE message_id, class Extension_type = Extension>
611 };
612 
614 template<MESSAGE_ID_TYPE message_id, class Extension_type = Extension>
615 class Message {
616  DEFINE_COMMON_CLASS(Message)
617 
618 public:
624  Message(uint8_t system_id, uint8_t component_id, uint32_t request_id, Io_buffer::Ptr buffer) :
625  payload(buffer),
626  sender_system_id(system_id),
627  sender_component_id(component_id),
628  sender_request_id(request_id) {}
629 
631  uint8_t
632  Get_sender_system_id() const
633  {
634  return sender_system_id;
635  }
636 
638  uint8_t
639  Get_sender_component_id() const
640  {
641  return sender_component_id;
642  }
643 
645  uint32_t
646  Get_sender_request_id() const
647  {
648  return sender_request_id;
649  }
650 
653 
654 private:
656  uint8_t sender_system_id;
657 
659  uint8_t sender_component_id;
660 
662  uint32_t sender_request_id;
663 };
664 
669 class Checksum {
670 public:
675 
677  Checksum();
678 
680  Checksum(const void* buffer, size_t len);
681 
683  Checksum(const Io_buffer::Ptr& buffer);
684 
692  uint16_t
693  Accumulate(const void* buffer, size_t len);
694 
701  uint16_t
702  Accumulate(const Io_buffer::Ptr& buffer);
703 
710  uint16_t
711  Accumulate(uint8_t byte);
712 
714  uint16_t
715  Get() const;
716 
724  static uint16_t
725  Calculate(const void* buffer, size_t len, uint16_t* accumulator = nullptr);
726 
737  static bool
739  MESSAGE_ID_TYPE message_id,
741  const Extension &ext = Extension::Get());
742 
744  void
745  Reset();
746 
747 private:
748  enum {
750  X25_INIT_CRC = 0xffff
751  };
752 
754  static void
755  Init(uint16_t& accumulator);
756 
758  uint16_t accumulator;
759 };
760 
761 } /* namespace mavlink */
762 } /* namespace vsm */
763 } /* namespace ugcs */
764 
765 /* Include files generated by mavgen.py. */
766 #include <ugcs/vsm/auto_mavlink_enums.h>
767 #include <ugcs/vsm/auto_mavlink_messages.h>
768 
769 #endif /* _UGCS_VSM_MAVLINK_H_ */
#define __PACKED
Pack structure or class, i.e.
Definition: defs.h:35
Helper class for byte-order-dependent value representation.
Definition: endian.h:345
#define VSM_DEFINE_DERIVED_EXCEPTION(__base_class, __exc_class)
Define custom derived exception.
Definition: exception.h:208
VSM definitions to work with byte order.
std::shared_ptr< Io_buffer > Ptr
Pointer type.
Definition: io_buffer.h:34
#define DEFINE_COMMON_CLASS(__class_name,...)
Use this macro to define some common attributes for a class.
Definition: utils.h:25
#define VSM_EXCEPTION(__exc_class, __msg,...)
Throw VSM exception instance.
Definition: exception.h:168
Io_buffer class implementation.
Helper class for defining derived exceptions.
Definition: exception.h:133
Base class for all VSM exceptions.
Definition: exception.h:22