VSM C++ SDK
Vehicle Specific Modules SDK
mavlink.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 
11 #ifndef MAVLINK_H_
12 #define 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,
39 };
40 
45  /* MAVLink standard types follow. */
46  CHAR,
47  INT8,
48  UINT8,
49  INT16,
50  UINT16,
51  INT32,
52  UINT32,
53  INT64,
54  UINT64,
55  FLOAT,
56  DOUBLE,
57  UINT8_VERSION,
58 
61 };
62 
64 template<class T, class = void>
66 };
67 
69 template<class T>
70 struct Field_default_value<T, typename std::enable_if<std::is_integral<T>::value>::type> {
72  static constexpr T value = std::numeric_limits<T>::max();
73 
75  static bool
76  Is_default(T val)
77  {
78  return val == value;
79  }
80 };
81 
83 template<class T>
84 struct Field_default_value<T, typename std::enable_if<std::is_floating_point<T>::value>::type> {
85  static constexpr T value = std::numeric_limits<T>::quiet_NaN();
86 
88  static bool
89  Is_default(T val)
90  {
91  return std::isnan(val);
92  }
93 };
94 
101 template <typename T, Field_type_id id, long initial_value = 0>
102 class Value {
103 public:
108  Value(T value = initial_value): value(value)
109  {}
110 
115  Value &
116  operator =(T value)
117  {
118  this->value = value;
119  return *this;
120  }
121 
126  operator T() const
127  {
128  return value;
129  }
130 
135  T
136  Get() const
137  {
138  return value;
139  }
140 
142  constexpr Field_type_id
144  {
145  return id;
146  }
147 
149  void
151  {
153  }
154 
156  bool
157  Is_reset() const
158  {
160  }
161 
162 private:
164  Le_value<T> value;
165 } __PACKED;
166 
167 // @{
170 typedef Value<int8_t, INT8> Int8;
178 typedef Value<float, FLOAT> Float;
181 // @}
182 
187 template <class TValue, size_t size>
188 class Value_array {
189 private:
191  TValue data[size];
192 public:
194  TValue &
195  operator [](size_t index)
196  {
197  if (index >= size) {
198  VSM_EXCEPTION(Invalid_param_exception, "Index out of range");
199  }
200  return data[index];
201  }
202 
204  void
206  {
207  for (auto& elem: data) {
208  elem.Reset();
209  }
210  }
211 } __PACKED;
212 
216 template <size_t size>
217 class Value_array<Char, size> {
218 private:
220  Char data[size];
221 public:
223  Char &
224  operator [](size_t index)
225  {
226  if (index >= size) {
227  VSM_EXCEPTION(Invalid_param_exception, "Index out of range");
228  }
229  return data[index];
230  }
231 
237  size_t
238  Get_length() const
239  {
240  size_t len = 0;
241  for (Char c: data) {
242  if (!c) {
243  return len;
244  }
245  len++;
246  }
247  return len;
248  }
249 
251  std::string
252  Get_string() const
253  {
254  return std::string(reinterpret_cast<const char *>(data), Get_length());
255  }
256 
258  void
260  {
261  for (auto& elem: data) {
262  elem = 0;
263  }
264  }
265 
266 #ifndef NO_DOXYGEN // Doxygen complains about such operator...
267 
268  operator std::string() const
269  {
270  return Get_string();
271  }
272 #endif
273 
275  bool
276  operator ==(const char *str)
277  {
278  return Get_string() == str;
279  }
280 
282  bool
283  operator !=(const char *str)
284  {
285  return Get_string() != str;
286  }
287 
294  Value_array &
295  operator =(const char *str)
296  {
297  size_t idx;
298  for (idx = 0; idx < size && *str; idx++, str++) {
299  data[idx] = *str;
300  }
301  if (idx < size) {
302  data[idx] = 0;
303  }
304  return *this;
305  }
306 
313  Value_array &
314  operator =(const std::string &str)
315  {
316  return *this = str.c_str();
317  }
318 
319 } __PACKED;
320 
321 namespace internal {
322 
328  const char *name;
332  size_t array_size;
333 };
334 
335 } /* namespace internal */
336 
339 typedef std::pair<uint8_t, uint16_t> Extra_byte_length_pair;
340 
344 typedef uint8_t MESSAGE_ID_TYPE;
345 
346 /* Forward declaration for a type which will be fully defined in generated headers. */
347 enum MESSAGE_ID: MESSAGE_ID_TYPE;
348 
352  typedef uint8_t System_id;
353 
355  typedef Uint8 System_id_wire;
356 };
357 
361  typedef uint32_t System_id;
362 
364  typedef Uint32 System_id_wire;
365 };
366 
370 typedef uint32_t System_id_common;
371 
373 class Extension {
374 public:
375 
377  virtual
378  ~Extension() {}
379 
381  static const Extension &
382  Get()
383  {
384  return instance;
385  }
386 
388  virtual std::string
389  Get_name() const
390  {
391  return std::string();
392  }
393 
395  virtual const std::map<MESSAGE_ID_TYPE, Extra_byte_length_pair> *
397  {
398  return &crc_extra_bytes_length_map;
399  }
400 private:
401  static const Extension instance;
405  static const std::map<MESSAGE_ID_TYPE, Extra_byte_length_pair> crc_extra_bytes_length_map;
406 };
407 
409 class Payload_base: public std::enable_shared_from_this<Payload_base> {
411 public:
412 
413  virtual
414  ~Payload_base()
415  {}
416 
418  virtual size_t
419  Get_size() const = 0;
420 
423  Get_buffer() const;
424 
426  std::string
427  Dump() const;
428 
430  virtual const char *
431  Get_name() const = 0;
432 
434  virtual MESSAGE_ID_TYPE
435  Get_id() const = 0;
436 
438  virtual uint8_t
439  Get_extra_byte() const = 0;
440 
442  virtual void
443  Reset() = 0;
444 
445 protected:
447  virtual const void *
448  Get_data() const = 0;
449 
456  Get_fields() const = 0;
457 };
458 
462 template <class TData, internal::Field_descriptor *fields, const char *msg_name,
463  MESSAGE_ID_TYPE msg_id, uint8_t extra_byte>
464 class Payload: public Payload_base {
466 public:
468  Payload() = default;
469 
479  Payload(const void *buf, size_t size)
480  {
481  if (size < sizeof(data)) {
482  VSM_EXCEPTION(Invalid_param_exception, "Too small buffer provided");
483  }
484  memcpy(&data, buf, sizeof(data));
485  }
486 
492  Payload(buffer->Get_data(), buffer->Get_length())
493  {}
494 
496  static constexpr size_t
498  {
499  return sizeof(TData);
500  }
501 
503  virtual size_t
504  Get_size() const override
505  {
506  return Get_payload_size();
507  }
508 
510  virtual const char *
511  Get_name() const override
512  {
513  return msg_name;
514  }
515 
517  virtual MESSAGE_ID_TYPE
518  Get_id() const override
519  {
520  return msg_id;
521  }
522 
524  virtual uint8_t
525  Get_extra_byte() const override
526  {
527  return extra_byte;
528  }
529 
531  virtual void
532  Reset() override
533  {
534  data.Reset();
535  }
536 
538  bool
539  operator ==(const Payload &rhs) const
540  {
541  return !memcmp(&data, &rhs.data, sizeof(data));
542  }
543 
545  bool
546  operator !=(const Payload &rhs) const
547  {
548  return memcmp(&data, &rhs.data, sizeof(data));
549  }
550 
552  TData *
553  operator ->()
554  {
555  return &data;
556  }
557 
559  const TData *
560  operator ->() const
561  {
562  return &data;
563  }
564 
566  TData &
567  operator *()
568  {
569  return data;
570  }
571 
573  const TData &
574  operator *() const
575  {
576  return data;
577  }
578 
579 private:
581  TData data;
582 
584  virtual const void *
585  Get_data() const override
586  {
587  return &data;
588  }
589 
596  Get_fields() const override
597  {
598  return fields;
599  }
600 };
601 
605 template <MESSAGE_ID_TYPE message_id, class Extension_type = Extension>
609 };
610 
612 template<MESSAGE_ID_TYPE message_id, class Extension_type = Extension>
613 class Message {
615 public:
616 
622  Message(System_id_common system_id, uint8_t component_id, uint32_t request_id, Io_buffer::Ptr buffer) :
623  payload(buffer),
624  sender_system_id(system_id),
625  sender_component_id(component_id),
626  sender_request_id(request_id){}
627 
629  System_id_common
631  {
632  return sender_system_id;
633  }
634 
636  uint8_t
638  {
639  return sender_component_id;
640  }
641 
643  uint32_t
645  {
646  return sender_request_id;
647  }
648 
651 
652 private:
653 
655  System_id_common sender_system_id;
656 
658  uint8_t sender_component_id;
659 
661  uint32_t sender_request_id;
662 };
663 
668 class Checksum {
669 public:
674 
676  Checksum();
677 
679  Checksum(const void* buffer, size_t len);
680 
682  Checksum(const Io_buffer::Ptr& buffer);
683 
691  uint16_t
692  Accumulate(const void* buffer, size_t len);
693 
700  uint16_t
701  Accumulate(const Io_buffer::Ptr& buffer);
702 
709  uint16_t
710  Accumulate(uint8_t byte);
711 
713  uint16_t
714  Get() const;
715 
723  static uint16_t
724  Calculate(const void* buffer, size_t len, uint16_t* accumulator = nullptr);
725 
736  static bool
737  Get_extra_byte_length_pair(
738  MESSAGE_ID_TYPE message_id,
739  Extra_byte_length_pair& ret,
740  const Extension &ext = Extension::Get());
741 
743  void
744  Reset();
745 
746 private:
747 
748  enum {
750  X25_INIT_CRC = 0xffff
751  };
752 
754  static void
755  Init(uint16_t& accumulator);
756 
758  uint16_t accumulator;
759 };
760 
762 template<typename Mavlink_kind>
763 struct Header {
765  Uint8 start_sign;
767  Uint8 payload_len;
769  Uint8 seq;
771  typename Mavlink_kind::System_id_wire system_id;
775  Uint8 message_id;
776 } __PACKED;
777 
778 } /* namespace mavlink */
779 
780 } /* namespace vsm */
781 } /* namespace ugcs */
782 
783 /* Include files generated by mavgen.py. */
784 #include <ugcs/vsm/auto_mavlink_enums.h>
785 #include <ugcs/vsm/auto_mavlink_messages.h>
786 
787 #endif /* MAVLINK_H_ */
UGCS root namespace.
Definition: android-linux/ugcs/vsm/platform_sockets.h:27
Exception class with one parameter.
Definition: exception.h:88
#define __PACKED
Pack structure or class, i.e.
Definition: defs.h:35
STL namespace.
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:210
VSM definitions to work with byte order.
#define VSM_DEFINE_EXCEPTION(__exc_class,...)
Define custom exception type.
Definition: exception.h:202
Generic I/O buffer.
Definition: io_buffer.h:33
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:170
Io_buffer class implementation.
Helper class for defining derived exceptions.
Definition: exception.h:135