msgmap.hpp
Go to the documentation of this file.
1 
2 #pragma once
3 
4 #include <endian.h>
5 #include <type_traits>
6 
7 namespace mavlink {
8 
12 class MsgMap {
13 public:
14 
15  explicit MsgMap(mavlink_message_t *p) :
16  msg(p), cmsg(p), pos(0)
17  { }
18 
19  explicit MsgMap(mavlink_message_t &p) :
20  msg(&p), cmsg(&p), pos(0)
21  { }
22 
23  explicit MsgMap(const mavlink_message_t *p) :
24  msg(nullptr), cmsg(p), pos(0)
25  { }
26 
27  inline void reset()
28  {
29  pos = 0;
30  }
31 
32  inline void reset(uint32_t msgid, uint8_t len)
33  {
34  assert(msg);
35 
36  msg->msgid = msgid; // necessary for finalize
37  msg->len = len; // needed only for deserialization w/o finalize
38  pos = 0;
39  }
40 
41  template<typename _T>
42  void operator<< (const _T data);
43 
44  template<class _T, size_t _Size>
45  void operator<< (const std::array<_T, _Size> &data);
46 
47  template<typename _T>
48  void operator>> (_T &data);
49 
50  template<class _T, size_t _Size>
51  void operator>> (std::array<_T, _Size> &data);
52 
53 private:
54  mavlink_message_t *msg; // for serialization
55  const mavlink_message_t *cmsg; // for deserialization
56  size_t pos;
57 
58  template<typename _T, typename _Tin>
59  inline void msg_swap_memcpy(_T &buf, _Tin data);
60 
61  template<typename _T, typename _Tout>
62  inline void cmsg_memcpy_bzero_swap_set_data(_T &buf, _Tout &data);
63 };
64 
65 } // namespace mavlink
66 
67 // implementation
68 
69 template<typename _T, typename _Tin>
71 {
72  if (std::is_floating_point<_Tin>::value) {
73  buf = *static_cast<const _T *>(static_cast<const void *>(&data));
74  } else {
75  buf = data;
76  }
77 
78  // htoleXX functions may be empty macros,
79  // switch will be optimized-out
80  switch (sizeof(_T)) {
81  case 2:
82  buf = htole16(buf);
83  break;
84 
85  case 4:
86  buf = htole32(buf);
87  break;
88 
89  case 8:
90  buf = htole64(buf);
91  break;
92 
93  default:
94  assert(false);
95  }
96 
97  memcpy(&_MAV_PAYLOAD_NON_CONST(msg)[pos], &buf, sizeof(buf));
98 }
99 
100 template<typename _T>
102 {
103  assert(msg);
104  assert(pos + sizeof(_T) <= MAVLINK_MAX_PAYLOAD_LEN);
105 
106  switch (sizeof(_T)) {
107  case 1:
109  break;
110 
111  case 2:
112  uint16_t data_le16;
113  msg_swap_memcpy(data_le16, data);
114  break;
115 
116  case 4:
117  uint32_t data_le32;
118  msg_swap_memcpy(data_le32, data);
119  break;
120 
121  case 8:
122  uint64_t data_le64;
123  msg_swap_memcpy(data_le64, data);
124  break;
125 
126  default:
127  assert(false);
128  }
129 
130  //std::cout << "M< s: " << sizeof(_T) << " p: " << pos << " d: " << data << std::endl;
131 
132  pos += sizeof(_T);
133 }
134 
135 template<class _T, size_t _Size>
136 void mavlink::MsgMap::operator<< (const std::array<_T, _Size> &data)
137 {
138  for (auto &v : data) {
139  *this << v;
140  }
141 }
142 
143 template<typename _T, typename _Tout>
145 {
146  memcpy(&buf, &_MAV_PAYLOAD(cmsg)[pos], sizeof(_T));
147 
148  // if message is trimmed - bzero tail
149  if (pos + sizeof(_T) > cmsg->len) {
150  union {
151  _T d;
152  uint8_t u8[sizeof(_T)];
153  } bz;
154 
155  size_t toclean = (pos + sizeof(_T)) - cmsg->len;
156  size_t start_pos = sizeof(_T) - toclean;
157 
158  //std::cout << "B> bzero s: " << sizeof(_T) << " c: " << toclean << " p: " << start_pos << std::endl;
159 
160  bz.d = buf;
161  memset(&bz.u8[start_pos], 0, toclean);
162  buf = bz.d;
163  }
164 
165  // leXXtoh functions may be empty macros,
166  // switch will be optimized-out
167  switch (sizeof(_T)) {
168  case 2:
169  buf = le16toh(buf);
170  break;
171 
172  case 4:
173  buf = le32toh(buf);
174  break;
175 
176  case 8:
177  buf = le64toh(buf);
178  break;
179 
180  default:
181  assert(false);
182  }
183 
184  if (std::is_floating_point<_Tout>::value) {
185  data = *static_cast<_Tout *>(static_cast<void *>(&buf));
186  } else {
187  data = buf;
188  }
189 }
190 
191 template<typename _T>
193 {
194  assert(cmsg);
195  assert(pos + sizeof(_T) <= MAVLINK_MAX_PAYLOAD_LEN);
196 
197  // message is trimmed - fill with zeroes
198  if (pos >= cmsg->len) {
199  data = 0;
200  pos += sizeof(_T);
201  return;
202  }
203 
204  switch (sizeof(_T)) {
205  case 1:
206  data = _MAV_PAYLOAD(cmsg)[pos];
207  break;
208 
209  case 2:
210  uint16_t data_le16;
211  cmsg_memcpy_bzero_swap_set_data(data_le16, data);
212  break;
213 
214  case 4:
215  uint32_t data_le32;
216  cmsg_memcpy_bzero_swap_set_data(data_le32, data);
217  break;
218 
219  case 8:
220  uint64_t data_le64;
221  cmsg_memcpy_bzero_swap_set_data(data_le64, data);
222  break;
223 
224  default:
225  assert(false);
226  }
227 
228  //std::cout << "M> s: " << sizeof(_T) << " p: " << pos << " d: " << data << std::endl;
229 
230  pos += sizeof(_T);
231 }
232 
233 template<class _T, size_t _Size>
234 void mavlink::MsgMap::operator>> (std::array<_T, _Size> &data)
235 {
236  for (auto &v : data) {
237  *this >> v;
238  }
239 }
240 


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Apr 7 2019 02:06:02