Go to most recent revision | Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1702 | - | 1 | // MESSAGE VISION_POSITION_ESTIMATE PACKING |
2 | |||
3 | #define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE 102 |
||
4 | |||
5 | typedef struct __mavlink_vision_position_estimate_t |
||
6 | { |
||
7 | uint64_t usec; ///< Timestamp (milliseconds) |
||
8 | float x; ///< Global X position |
||
9 | float y; ///< Global Y position |
||
10 | float z; ///< Global Z position |
||
11 | float roll; ///< Roll angle in rad |
||
12 | float pitch; ///< Pitch angle in rad |
||
13 | float yaw; ///< Yaw angle in rad |
||
14 | } mavlink_vision_position_estimate_t; |
||
15 | |||
16 | #define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN 32 |
||
17 | #define MAVLINK_MSG_ID_102_LEN 32 |
||
18 | |||
19 | |||
20 | |||
21 | #define MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE { \ |
||
22 | "VISION_POSITION_ESTIMATE", \ |
||
23 | 7, \ |
||
24 | { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_position_estimate_t, usec) }, \ |
||
25 | { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_position_estimate_t, x) }, \ |
||
26 | { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_position_estimate_t, y) }, \ |
||
27 | { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_position_estimate_t, z) }, \ |
||
28 | { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vision_position_estimate_t, roll) }, \ |
||
29 | { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vision_position_estimate_t, pitch) }, \ |
||
30 | { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vision_position_estimate_t, yaw) }, \ |
||
31 | } \ |
||
32 | } |
||
33 | |||
34 | |||
35 | /** |
||
36 | * @brief Pack a vision_position_estimate message |
||
37 | * @param system_id ID of this system |
||
38 | * @param component_id ID of this component (e.g. 200 for IMU) |
||
39 | * @param msg The MAVLink message to compress the data into |
||
40 | * |
||
41 | * @param usec Timestamp (milliseconds) |
||
42 | * @param x Global X position |
||
43 | * @param y Global Y position |
||
44 | * @param z Global Z position |
||
45 | * @param roll Roll angle in rad |
||
46 | * @param pitch Pitch angle in rad |
||
47 | * @param yaw Yaw angle in rad |
||
48 | * @return length of the message in bytes (excluding serial stream start sign) |
||
49 | */ |
||
50 | static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, |
||
51 | uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) |
||
52 | { |
||
53 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS |
||
54 | char buf[32]; |
||
55 | _mav_put_uint64_t(buf, 0, usec); |
||
56 | _mav_put_float(buf, 8, x); |
||
57 | _mav_put_float(buf, 12, y); |
||
58 | _mav_put_float(buf, 16, z); |
||
59 | _mav_put_float(buf, 20, roll); |
||
60 | _mav_put_float(buf, 24, pitch); |
||
61 | _mav_put_float(buf, 28, yaw); |
||
62 | |||
63 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32); |
||
64 | #else |
||
65 | mavlink_vision_position_estimate_t packet; |
||
66 | packet.usec = usec; |
||
67 | packet.x = x; |
||
68 | packet.y = y; |
||
69 | packet.z = z; |
||
70 | packet.roll = roll; |
||
71 | packet.pitch = pitch; |
||
72 | packet.yaw = yaw; |
||
73 | |||
74 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32); |
||
75 | #endif |
||
76 | |||
77 | msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; |
||
78 | return mavlink_finalize_message(msg, system_id, component_id, 32, 158); |
||
79 | } |
||
80 | |||
81 | /** |
||
82 | * @brief Pack a vision_position_estimate message on a channel |
||
83 | * @param system_id ID of this system |
||
84 | * @param component_id ID of this component (e.g. 200 for IMU) |
||
85 | * @param chan The MAVLink channel this message was sent over |
||
86 | * @param msg The MAVLink message to compress the data into |
||
87 | * @param usec Timestamp (milliseconds) |
||
88 | * @param x Global X position |
||
89 | * @param y Global Y position |
||
90 | * @param z Global Z position |
||
91 | * @param roll Roll angle in rad |
||
92 | * @param pitch Pitch angle in rad |
||
93 | * @param yaw Yaw angle in rad |
||
94 | * @return length of the message in bytes (excluding serial stream start sign) |
||
95 | */ |
||
96 | static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, |
||
97 | mavlink_message_t* msg, |
||
98 | uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) |
||
99 | { |
||
100 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS |
||
101 | char buf[32]; |
||
102 | _mav_put_uint64_t(buf, 0, usec); |
||
103 | _mav_put_float(buf, 8, x); |
||
104 | _mav_put_float(buf, 12, y); |
||
105 | _mav_put_float(buf, 16, z); |
||
106 | _mav_put_float(buf, 20, roll); |
||
107 | _mav_put_float(buf, 24, pitch); |
||
108 | _mav_put_float(buf, 28, yaw); |
||
109 | |||
110 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32); |
||
111 | #else |
||
112 | mavlink_vision_position_estimate_t packet; |
||
113 | packet.usec = usec; |
||
114 | packet.x = x; |
||
115 | packet.y = y; |
||
116 | packet.z = z; |
||
117 | packet.roll = roll; |
||
118 | packet.pitch = pitch; |
||
119 | packet.yaw = yaw; |
||
120 | |||
121 | memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32); |
||
122 | #endif |
||
123 | |||
124 | msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; |
||
125 | return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 158); |
||
126 | } |
||
127 | |||
128 | /** |
||
129 | * @brief Encode a vision_position_estimate struct into a message |
||
130 | * |
||
131 | * @param system_id ID of this system |
||
132 | * @param component_id ID of this component (e.g. 200 for IMU) |
||
133 | * @param msg The MAVLink message to compress the data into |
||
134 | * @param vision_position_estimate C-struct to read the message contents from |
||
135 | */ |
||
136 | static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) |
||
137 | { |
||
138 | return mavlink_msg_vision_position_estimate_pack(system_id, component_id, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); |
||
139 | } |
||
140 | |||
141 | /** |
||
142 | * @brief Send a vision_position_estimate message |
||
143 | * @param chan MAVLink channel to send the message |
||
144 | * |
||
145 | * @param usec Timestamp (milliseconds) |
||
146 | * @param x Global X position |
||
147 | * @param y Global Y position |
||
148 | * @param z Global Z position |
||
149 | * @param roll Roll angle in rad |
||
150 | * @param pitch Pitch angle in rad |
||
151 | * @param yaw Yaw angle in rad |
||
152 | */ |
||
153 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS |
||
154 | |||
155 | static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) |
||
156 | { |
||
157 | #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS |
||
158 | char buf[32]; |
||
159 | _mav_put_uint64_t(buf, 0, usec); |
||
160 | _mav_put_float(buf, 8, x); |
||
161 | _mav_put_float(buf, 12, y); |
||
162 | _mav_put_float(buf, 16, z); |
||
163 | _mav_put_float(buf, 20, roll); |
||
164 | _mav_put_float(buf, 24, pitch); |
||
165 | _mav_put_float(buf, 28, yaw); |
||
166 | |||
167 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, 32, 158); |
||
168 | #else |
||
169 | mavlink_vision_position_estimate_t packet; |
||
170 | packet.usec = usec; |
||
171 | packet.x = x; |
||
172 | packet.y = y; |
||
173 | packet.z = z; |
||
174 | packet.roll = roll; |
||
175 | packet.pitch = pitch; |
||
176 | packet.yaw = yaw; |
||
177 | |||
178 | _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)&packet, 32, 158); |
||
179 | #endif |
||
180 | } |
||
181 | |||
182 | #endif |
||
183 | |||
184 | // MESSAGE VISION_POSITION_ESTIMATE UNPACKING |
||
185 | |||
186 | |||
187 | /** |
||
188 | * @brief Get field usec from vision_position_estimate message |
||
189 | * |
||
190 | * @return Timestamp (milliseconds) |
||
191 | */ |
||
192 | static inline uint64_t mavlink_msg_vision_position_estimate_get_usec(const mavlink_message_t* msg) |
||
193 | { |
||
194 | return _MAV_RETURN_uint64_t(msg, 0); |
||
195 | } |
||
196 | |||
197 | /** |
||
198 | * @brief Get field x from vision_position_estimate message |
||
199 | * |
||
200 | * @return Global X position |
||
201 | */ |
||
202 | static inline float mavlink_msg_vision_position_estimate_get_x(const mavlink_message_t* msg) |
||
203 | { |
||
204 | return _MAV_RETURN_float(msg, 8); |
||
205 | } |
||
206 | |||
207 | /** |
||
208 | * @brief Get field y from vision_position_estimate message |
||
209 | * |
||
210 | * @return Global Y position |
||
211 | */ |
||
212 | static inline float mavlink_msg_vision_position_estimate_get_y(const mavlink_message_t* msg) |
||
213 | { |
||
214 | return _MAV_RETURN_float(msg, 12); |
||
215 | } |
||
216 | |||
217 | /** |
||
218 | * @brief Get field z from vision_position_estimate message |
||
219 | * |
||
220 | * @return Global Z position |
||
221 | */ |
||
222 | static inline float mavlink_msg_vision_position_estimate_get_z(const mavlink_message_t* msg) |
||
223 | { |
||
224 | return _MAV_RETURN_float(msg, 16); |
||
225 | } |
||
226 | |||
227 | /** |
||
228 | * @brief Get field roll from vision_position_estimate message |
||
229 | * |
||
230 | * @return Roll angle in rad |
||
231 | */ |
||
232 | static inline float mavlink_msg_vision_position_estimate_get_roll(const mavlink_message_t* msg) |
||
233 | { |
||
234 | return _MAV_RETURN_float(msg, 20); |
||
235 | } |
||
236 | |||
237 | /** |
||
238 | * @brief Get field pitch from vision_position_estimate message |
||
239 | * |
||
240 | * @return Pitch angle in rad |
||
241 | */ |
||
242 | static inline float mavlink_msg_vision_position_estimate_get_pitch(const mavlink_message_t* msg) |
||
243 | { |
||
244 | return _MAV_RETURN_float(msg, 24); |
||
245 | } |
||
246 | |||
247 | /** |
||
248 | * @brief Get field yaw from vision_position_estimate message |
||
249 | * |
||
250 | * @return Yaw angle in rad |
||
251 | */ |
||
252 | static inline float mavlink_msg_vision_position_estimate_get_yaw(const mavlink_message_t* msg) |
||
253 | { |
||
254 | return _MAV_RETURN_float(msg, 28); |
||
255 | } |
||
256 | |||
257 | /** |
||
258 | * @brief Decode a vision_position_estimate message into a struct |
||
259 | * |
||
260 | * @param msg The message to decode |
||
261 | * @param vision_position_estimate C-struct to decode the message contents into |
||
262 | */ |
||
263 | static inline void mavlink_msg_vision_position_estimate_decode(const mavlink_message_t* msg, mavlink_vision_position_estimate_t* vision_position_estimate) |
||
264 | { |
||
265 | #if MAVLINK_NEED_BYTE_SWAP |
||
266 | vision_position_estimate->usec = mavlink_msg_vision_position_estimate_get_usec(msg); |
||
267 | vision_position_estimate->x = mavlink_msg_vision_position_estimate_get_x(msg); |
||
268 | vision_position_estimate->y = mavlink_msg_vision_position_estimate_get_y(msg); |
||
269 | vision_position_estimate->z = mavlink_msg_vision_position_estimate_get_z(msg); |
||
270 | vision_position_estimate->roll = mavlink_msg_vision_position_estimate_get_roll(msg); |
||
271 | vision_position_estimate->pitch = mavlink_msg_vision_position_estimate_get_pitch(msg); |
||
272 | vision_position_estimate->yaw = mavlink_msg_vision_position_estimate_get_yaw(msg); |
||
273 | #else |
||
274 | memcpy(vision_position_estimate, _MAV_PAYLOAD(msg), 32); |
||
275 | #endif |
||
276 | } |