VSM C++ SDK
Vehicle Specific Modules SDK
mavlink_encoder.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 
8 #ifndef _MAVLINK_ENCODER_H_
9 #define _MAVLINK_ENCODER_H_
10 
11 #include <ugcs/vsm/io_buffer.h>
12 #include <ugcs/vsm/mavlink.h>
13 
14 namespace ugcs {
15 namespace vsm {
16 
21 public:
28  template<typename Mavlink_kind>
30  Encode(const mavlink::Payload_base& payload,
31  typename Mavlink_kind::System_id system_id, uint8_t component_id)
32  {
33  using Header = mavlink::Header<Mavlink_kind>;
34 
35  /* Reserve space for the whole packet. */
36  std::vector<uint8_t> data(sizeof(Header) + payload.Get_size() +
37  sizeof(uint16_t));
38 
39  Header* hdr = reinterpret_cast<Header*>(data.data());
40 
41  /* Fill the header. */
42  hdr->start_sign = mavlink::START_SIGN;
43  hdr->payload_len = payload.Get_size();
44  hdr->seq = seq++;
45  hdr->system_id = system_id;
46  hdr->component_id = component_id;
47  hdr->message_id = static_cast<uint8_t>(payload.Get_id());
48 
49  /* If this affects performance, then Payload class should be redesigned
50  * to provide space for header and checksum in-place.
51  */
52  memcpy(&data[sizeof(Header)], payload.Get_buffer()->Get_data(),
53  payload.Get_size());
54 
55  /* Don't include start sign. */
56  mavlink::Checksum sum(&data[1], sizeof(Header) + payload.Get_size() - 1);
57  uint16_t sum_val = sum.Accumulate(payload.Get_extra_byte());
58  mavlink::Uint16 *wire_sum =
59  reinterpret_cast<mavlink::Uint16*>(&data[sizeof(Header) + payload.Get_size()]);
60  *wire_sum = sum_val;
61 
62  return Io_buffer::Create(std::move(data));
63  }
64 
72  template<typename Mavlink_kind>
75  const mavlink::Payload_base& payload,
76  typename Mavlink_kind::System_id system_id,
77  uint8_t component_id,
78  uint32_t request_id)
79  {
80  using Header = mavlink::Header<Mavlink_kind>;
81 
82  /* Reserve space for the whole packet. */
83  std::vector<uint8_t> data(sizeof(Header) + payload.Get_size() +
84  sizeof(uint16_t));
85 
86  Header* hdr = reinterpret_cast<Header*>(data.data());
87 
88  /* Fill the header. */
89  hdr->start_sign = mavlink::START_SIGN;
90  hdr->payload_len = payload.Get_size();
91  hdr->seq = request_id;
92  hdr->system_id = system_id;
93  hdr->component_id = component_id;
94  hdr->message_id = static_cast<uint8_t>(payload.Get_id());
95 
96  /* If this affects performance, then Payload class should be redesigned
97  * to provide space for header and checksum in-place.
98  */
99  memcpy(&data[sizeof(Header)], payload.Get_buffer()->Get_data(),
100  payload.Get_size());
101 
102  /* Don't include start sign. */
103  mavlink::Checksum sum(&data[1], sizeof(Header) + payload.Get_size() - 1);
104  uint16_t sum_val = sum.Accumulate(payload.Get_extra_byte());
105  mavlink::Uint16 *wire_sum =
106  reinterpret_cast<mavlink::Uint16*>(&data[sizeof(Header) + payload.Get_size()]);
107  *wire_sum = sum_val;
108 
109  return Io_buffer::Create(std::move(data));
110  }
111 private:
113  uint8_t seq = 0;
114 };
115 
116 } /* namespace vsm */
117 } /* namespace ugcs */
118 
119 #endif /* _MAVLINK_ENCODER_H_ */
UGCS root namespace.
Definition: android-linux/ugcs/vsm/platform_sockets.h:27
static Ptr Create(Args &&...args)
Create an instance.
Definition: io_buffer.h:34
std::shared_ptr< Io_buffer > Ptr
Pointer type.
Definition: io_buffer.h:34
Io_buffer class implementation.