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 
21 namespace ugcs {
22 namespace vsm {
23 
25 namespace mavlink {
26 
27 enum {
29  VERSION = 3,
31  START_SIGN = 0xfe,
33  START_SIGN2 = 0xfd,
38 
41 };
42 
43 static constexpr size_t MAVLINK_1_HEADER_LEN = 6;
44 static constexpr size_t MAVLINK_1_MIN_FRAME_LEN = MAVLINK_1_HEADER_LEN + 2;
45 
46 static constexpr size_t MAVLINK_2_HEADER_LEN = 10;
47 static constexpr size_t MAVLINK_2_MIN_FRAME_LEN = MAVLINK_2_HEADER_LEN + 2;
48 
53  /* MAVLink standard types follow. */
54  CHAR,
55  INT8,
56  UINT8,
57  INT16,
58  UINT16,
59  INT32,
60  UINT32,
61  INT64,
62  UINT64,
63  FLOAT,
64  DOUBLE,
65  UINT8_VERSION,
66 
69 };
70 
72 template<class T, class = void>
74 };
75 
77 template<class T>
78 struct Field_default_value<T, typename std::enable_if<std::is_integral<T>::value>::type> {
80  static constexpr T value = std::numeric_limits<T>::max();
81 
83  static bool
84  Is_default(T val)
85  {
86  return val == value;
87  }
88 };
89 
91 template<class T>
92 struct Field_default_value<T, typename std::enable_if<std::is_floating_point<T>::value>::type> {
93  static constexpr T value = std::numeric_limits<T>::quiet_NaN();
94 
96  static bool
97  Is_default(T val)
98  {
99  return std::isnan(val);
100  }
101 };
109 template <typename T, Field_type_id id, int32_t initial_value = 0>
110 class Value {
111 public:
116  Value(T value = initial_value): value(value)
117  {}
118 
123  Value &
124  operator =(T value)
125  {
126  this->value = value;
127  return *this;
128  }
129 
134  operator T() const
135  {
136  return value;
137  }
138 
143  T
144  Get() const
145  {
146  return value;
147  }
148 
150  constexpr Field_type_id
152  {
153  return id;
154  }
155 
157  void
159  {
161  }
162 
164  bool
165  Is_reset() const
166  {
168  }
169 
170 private:
172  Le_value<T> value;
173 } __PACKED;
174 
175 // @{
178 typedef Value<int8_t, INT8> Int8;
186 typedef Value<float, FLOAT> Float;
189 // @}
190 
195 template <class TValue, size_t size>
196 class Value_array {
197 private:
199  TValue data[size];
200 public:
202  TValue &
203  operator[](size_t index)
204  {
205  if (index >= size) {
206  VSM_EXCEPTION(Invalid_param_exception, "Index out of range");
207  }
208  return data[index];
209  }
212  void
214  {
215  for (auto& elem : data) {
216  elem.Reset();
217  }
218  }
219 } __PACKED;
220 
224 template <size_t size>
225 class Value_array<Char, size> {
226 private:
228  Char data[size];
229 
230 public:
232  Char &
233  operator[](size_t index)
234  {
235  if (index >= size) {
236  VSM_EXCEPTION(Invalid_param_exception, "Index out of range");
237  }
238  return data[index];
239  }
240 
246  size_t
247  Get_length() const
248  {
249  size_t len = 0;
250  for (Char c : data) {
251  if (!c) {
252  return len;
253  }
254  len++;
255  }
256  return len;
257  }
258 
260  std::string
261  Get_string() const
262  {
263  return std::string(reinterpret_cast<const char *>(data), Get_length());
264  }
265 
267  void
269  {
270  for (auto& elem : data) {
271  elem = 0;
272  }
273  }
274 
275 #ifndef NO_DOXYGEN // Doxygen complains about such operator...
276 
277  operator std::string() const
278  {
279  return Get_string();
280  }
281 #endif
282 
284  bool
285  operator ==(const char *str)
286  {
287  return Get_string() == str;
288  }
289 
291  bool
292  operator !=(const char *str)
293  {
294  return Get_string() != str;
295  }
296 
303  Value_array &
304  operator =(const char *str)
305  {
306  size_t idx;
307  for (idx = 0; idx < size && *str; idx++, str++) {
308  data[idx] = *str;
309  }
310  if (idx < size) {
311  data[idx] = 0;
312  }
313  return *this;
314  }
315 
322  Value_array &
323  operator =(const std::string &str)
324  {
325  return *this = str.c_str();
326  }
327 } __PACKED;
328 
329 namespace internal {
330 
336  const char *name;
340  size_t array_size;
341 };
342 
343 } /* namespace internal */
344 
347 typedef std::pair<uint32_t, uint16_t> Extra_byte_length_pair;
348 
352 typedef uint32_t MESSAGE_ID_TYPE;
353 
354 /* Forward declaration for a type which will be fully defined in generated headers. */
355 enum MESSAGE_ID: MESSAGE_ID_TYPE;
356 
358 class Extension {
359 public:
361  virtual
363 
365  static const Extension &
366  Get()
367  {
368  return instance;
369  }
370 
372  virtual std::string
373  Get_name() const
374  {
375  return std::string();
376  }
377 
379  virtual const std::map<MESSAGE_ID_TYPE, Extra_byte_length_pair> *
381  {
382  return &crc_extra_bytes_length_map;
383  }
384 
385 private:
386  static const Extension instance;
390  static const std::map<MESSAGE_ID_TYPE, Extra_byte_length_pair> crc_extra_bytes_length_map;
391 };
392 
394 class Payload_base: public std::enable_shared_from_this<Payload_base> {
396 
397 public:
398  virtual
399  ~Payload_base()
400  {}
401 
403  virtual size_t
404  Get_size_v1() const = 0;
405 
407  virtual size_t
408  Get_size_v2() const = 0;
409 
412  Get_buffer() const;
413 
415  std::string
416  Dump() const;
417 
419  virtual const char *
420  Get_name() const = 0;
421 
423  virtual MESSAGE_ID_TYPE
424  Get_id() const = 0;
425 
427  virtual uint8_t
428  Get_extra_byte() const = 0;
429 
431  virtual void
432  Reset() = 0;
433 
434 protected:
436  virtual const void *
437  Get_data() const = 0;
438 
445  Get_fields() const = 0;
446 };
447 
448 typedef std::vector<Payload_base::Ptr> Payload_list;
449 
453 template <class TData, internal::Field_descriptor *fields, const char *msg_name,
454  MESSAGE_ID_TYPE msg_id, uint8_t extra_byte>
455 class Payload: public Payload_base {
457 
458 public:
460  Payload() = default;
461 
471  Payload(const void *buf, size_t size)
472  {
473  if (size > sizeof(data)) {
474  size = sizeof(data);
475  } else if (size < sizeof(data)) {
476  memset(&data, 0, sizeof(data));
477  }
478  memcpy(&data, buf, size);
479  }
480 
486  Payload(buffer->Get_data(), buffer->Get_length())
487  {}
488 
490  virtual size_t
491  Get_size_v1() const override
492  {
493  return data.Get_size_v1();
494  }
495 
497  virtual size_t
498  Get_size_v2() const override
499  {
500  return data.Get_size_v2();
501  }
502 
504  virtual const char *
505  Get_name() const override
506  {
507  return msg_name;
508  }
509 
511  virtual MESSAGE_ID_TYPE
512  Get_id() const override
513  {
514  return msg_id;
515  }
516 
518  virtual uint8_t
519  Get_extra_byte() const override
520  {
521  return extra_byte;
522  }
523 
525  virtual void
526  Reset() override
527  {
528  data.Reset();
529  }
530 
532  bool
533  operator ==(const Payload &rhs) const
534  {
535  return !memcmp(&data, &rhs.data, sizeof(data));
536  }
537 
539  bool
540  operator !=(const Payload &rhs) const
541  {
542  return memcmp(&data, &rhs.data, sizeof(data));
543  }
544 
546  TData *
548  {
549  return &data;
550  }
551 
553  const TData *
554  operator ->() const
555  {
556  return &data;
557  }
558 
560  TData &
562  {
563  return data;
564  }
565 
567  const TData &
568  operator *() const
569  {
570  return data;
571  }
572 
573 private:
575  TData data;
576 
578  virtual const void *
579  Get_data() const override
580  {
581  return &data;
582  }
583 
589  virtual internal::Field_descriptor *
590  Get_fields() const override
591  {
592  return fields;
593  }
594 };
595 
599 template <MESSAGE_ID_TYPE message_id, class Extension_type = Extension>
603 };
604 
606 template<MESSAGE_ID_TYPE message_id, class Extension_type = Extension>
607 class Message {
608  DEFINE_COMMON_CLASS(Message)
609 
610 public:
616  Message(uint8_t system_id, uint8_t component_id, uint32_t request_id, Io_buffer::Ptr buffer) :
617  payload(buffer),
618  sender_system_id(system_id),
619  sender_component_id(component_id),
620  sender_request_id(request_id) {}
621 
623  uint8_t
624  Get_sender_system_id() const
625  {
626  return sender_system_id;
627  }
628 
630  uint8_t
631  Get_sender_component_id() const
632  {
633  return sender_component_id;
634  }
635 
637  uint32_t
638  Get_sender_request_id() const
639  {
640  return sender_request_id;
641  }
642 
645 
646 private:
648  uint8_t sender_system_id;
649 
651  uint8_t sender_component_id;
652 
654  uint32_t sender_request_id;
655 };
656 
661 class Checksum {
662 public:
667 
669  Checksum();
670 
672  Checksum(const void* buffer, size_t len);
673 
675  Checksum(const Io_buffer::Ptr& buffer);
676 
684  uint16_t
685  Accumulate(const void* buffer, size_t len);
686 
693  uint16_t
694  Accumulate(const Io_buffer::Ptr& buffer);
695 
702  uint16_t
703  Accumulate(uint8_t byte);
704 
706  uint16_t
707  Get() const;
708 
716  static uint16_t
717  Calculate(const void* buffer, size_t len, uint16_t* accumulator = nullptr);
718 
729  static bool
731  MESSAGE_ID_TYPE message_id,
733  const Extension &ext = Extension::Get());
734 
736  void
737  Reset();
738 
739 private:
740  enum {
742  X25_INIT_CRC = 0xffff
743  };
744 
746  static void
747  Init(uint16_t& accumulator);
748 
750  uint16_t accumulator;
751 };
752 
753 } /* namespace mavlink */
754 } /* namespace vsm */
755 } /* namespace ugcs */
756 
757 /* Include files generated by mavgen.py. */
758 #include <ugcs/vsm/auto_mavlink_enums.h>
759 #include <ugcs/vsm/auto_mavlink_messages.h>
760 
761 #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:325
#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