VSM C++ SDK
Vehicle Specific Modules SDK
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
mavlink_demuxer.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_DEMUXER_H_
12 #define _UGCS_VSM_MAVLINK_DEMUXER_H_
13 
16 #include <unordered_map>
17 
18 namespace ugcs {
19 namespace vsm {
20 
27 public:
30  typedef signed Message_id;
31 
33  static constexpr Message_id MESSAGE_ID_ANY = -1;
34 
37  typedef int64_t System_id;
38 
40  static constexpr System_id SYSTEM_ID_ANY = -1;
41 
44  typedef signed Component_id;
45 
47  static constexpr Component_id COMPONENT_ID_ANY = -1;
48 
50  VSM_DEFINE_EXCEPTION(Duplicate_handler);
51 
53  template<mavlink::MESSAGE_ID_TYPE message_id, class Extention_type = mavlink::Extension>
54  using Handler = Callback_proxy<
56 
65 
67  DEFINE_CALLBACK_BUILDER(Make_default_handler,
68  (Io_buffer::Ptr, mavlink::MESSAGE_ID_TYPE, System_id, uint8_t, uint32_t),
69  (Io_buffer::Ptr(nullptr), mavlink::MESSAGE_ID::DEBUG_VALUE, mavlink::SYSTEM_ID_NONE, 0, 0))
70 
71 
73  (mavlink::MESSAGE_ID_TYPE message_id, class Extention_type),
74  (typename mavlink::Message<message_id, Extention_type>::Ptr),
75  (typename mavlink::Message<message_id, Extention_type>::Ptr(nullptr)))
76 
78  class Key {
79  public:
81  Key(mavlink::MESSAGE_ID_TYPE message_id, System_id system_id, Component_id component_id) :
82  message_id(message_id),
83  system_id(system_id),
84  component_id(component_id) {
85  }
86 
88  Key() {}
89 
91  class Hasher {
92  public:
94  size_t
95  operator()(const Key& key) const
96  {
97  return key.message_id ^ (key.system_id << 16) ^ (key.component_id << 24);
98  }
99  };
100 
102  void
104  {
105  id = 0;
106  }
107 
109  explicit operator bool() const
110  {
111  return id != 0;
112  }
113 
115  bool
116  operator ==(const Key& key) const
117  {
118  return this->message_id == key.message_id &&
119  this->system_id == key.system_id &&
120  this->component_id == key.component_id;
121  }
122 
123  private:
124  mavlink::MESSAGE_ID_TYPE message_id;
125 
126  System_id system_id;
127 
128  Component_id component_id;
129 
130  static std::atomic_int generator;
131  int id = 0;
132 
133  // used only when inserting into handlers.
134  void
135  Generate_id()
136  {
137  id = std::atomic_fetch_add(&Key::generator, 1);
138  }
139 
140  friend class Mavlink_demuxer;
141  };
142 
144  Mavlink_demuxer() = default;
145 
147  Mavlink_demuxer(const Mavlink_demuxer&) = delete;
148 
150  void
151  Disable();
152 
155  void
156  Register_default_handler(Default_handler handler);
157 
171  template<mavlink::MESSAGE_ID_TYPE message_id, class Extention_type>
172  Key
173  Register_handler(
174  Handler<message_id, Extention_type> handler,
175  System_id system_id = SYSTEM_ID_ANY,
176  Component_id component_id = COMPONENT_ID_ANY,
177  Request_processor::Ptr processor = nullptr)
178  {
180  handler, processor);
181  Key key(message_id, system_id, component_id);
182  key.Generate_id();
183  std::unique_lock<std::mutex> lock(mutex);
184  handlers.insert(std::make_pair(key, std::move(callback)));
185  return key;
186  }
187 
192  bool
193  Demux(Io_buffer::Ptr buffer, mavlink::MESSAGE_ID_TYPE message_id,
194  System_id system_id, uint8_t component_id, uint32_t request_id);
195 
198  void
199  Unregister_handler(Key&);
200 
201 private:
206  bool
207  Demux_try(Io_buffer::Ptr buffer, mavlink::MESSAGE_ID_TYPE message_id,
208  System_id system_id, uint8_t component_id, uint32_t request_id);
209 
214  bool
215  Demux_try_one(Io_buffer::Ptr buffer, mavlink::MESSAGE_ID_TYPE message_id,
216  System_id system_id, Component_id component_id,
217  System_id real_system_id,
218  uint8_t real_component_id,
219  uint32_t request_id);
220 
225  class Callback_base: public std::enable_shared_from_this<Callback_base> {
226  DEFINE_COMMON_CLASS(Callback_base, Callback_base)
227  public:
228  Callback_base(Request_processor::Ptr processor) :
229  processor(processor) {}
230 
231  virtual
232  ~Callback_base()
233  {};
234 
235  virtual void
236  operator()(Io_buffer::Ptr buffer, System_id system_id,
237  uint8_t component_id, uint32_t request_id) = 0;
238 
239  protected:
241  Request_processor::Ptr processor;
242  };
243 
245  template<mavlink::MESSAGE_ID_TYPE message_id, class Extention_type>
246  class Callback: public Callback_base {
247  DEFINE_COMMON_CLASS(Callback, Callback_base)
248 
249  public:
251  using Message_type = mavlink::Message<message_id, Extention_type>;
252 
253  Callback(Handler<message_id, Extention_type> handler,
254  Request_processor::Ptr processor):
255  Callback_base(processor),
256  handler(handler)
257  {}
258 
259  virtual void
260  operator()(Io_buffer::Ptr buffer, System_id system_id,
261  uint8_t component_id, uint32_t request_id) override
262  {
263  typename Message_type::Ptr message =
264  Message_type::Create(system_id, component_id, request_id, buffer);
265  if (processor) {
266  /* Callback will be invoked from processor context. */
267  auto request = Request::Create();
268  request->Set_processing_handler(Make_callback(&Callback::Invoke,
269  Shared_from_this(), std::move(message), request));
270  processor->Submit_request(std::move(request));
271 
272  } else {
273  /* Invoke from the calling thread. */
274  handler(message);
275  }
276  }
277 
278  private:
280  Handler<message_id, Extention_type> handler;
281 
283  void
284  Invoke(typename Message_type::Ptr message, Request::Ptr request)
285  {
286  handler(message);
287  if (request) {
288  request->Complete();
289  }
290  }
291  };
292 
294  Default_handler default_handler;
295 
297  std::unordered_multimap<Key, Callback_base::Ptr, Key::Hasher> handlers;
298 
300  std::mutex mutex;
301 };
302 
303 } /* namespace vsm */
304 } /* namespace ugcs */
305 
306 #endif /* _UGCS_VSM_MAVLINK_DEMUXER_H_ */
static Ptr Create(Args &&...args)
Create an instance.
Definition: request_container.h:38
Base_type::Result_t Invoke(callback_internal::Sequence< s...>)
Invoke callable entity with user provided arguments.
Definition: callback.h:283
#define DEFINE_CALLBACK_BUILDER(__name, __types, __values)
Define callback builder function.
Definition: callback.h:42
Request_context< true > Request_processor
Request processor is a request execution context where request are processed.
Definition: request_context.h:50
#define DEFINE_CALLBACK_BUILDER_TEMPLATE(__name, __template, __types, __values)
The same as DEFINE_CALLBACK_BUILDER, but allows specifying additional template parameters for builder...
Definition: callback.h:55
std::shared_ptr< Request > Ptr
Pointer type.
Definition: request_container.h:38
Request execution context.
Helper class for proxying callback invocation.
Definition: callback.h:694
Callback< Callable, void, Args...>::Ptr Make_callback(Callable &&callable, Args &&...args)
Create a callback.
Definition: callback.h:389
std::shared_ptr< Request_context > Ptr
Pointer type.
Definition: request_context.h:25
static Ptr Create(Callable &&callable, Args &&...args)
Create callback instance.
Definition: callback.h:235
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