mavlink_msg_optical_flow.h 8.87 KB
Newer Older
1 2 3 4
// MESSAGE OPTICAL_FLOW PACKING

#define MAVLINK_MSG_ID_OPTICAL_FLOW 100

lm's avatar
lm committed
5
typedef struct __mavlink_optical_flow_t
6
{
7
 uint64_t time_usec; ///< Timestamp (UNIX)
8 9 10
 float flow_x; ///< Flow in x-sensor direction
 float flow_y; ///< Flow in y-sensor direction
 float ground_distance; ///< Ground distance in meters (-1 for unknown, flow is then in pixel / arbitrary scale)
lm's avatar
lm committed
11 12
 uint8_t sensor_id; ///< Sensor ID
 uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality
13 14
} mavlink_optical_flow_t;

15 16
#define MAVLINK_MSG_ID_OPTICAL_FLOW_LEN 22
#define MAVLINK_MSG_ID_100_LEN 22
lm's avatar
lm committed
17 18 19 20 21 22



#define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW { \
	"OPTICAL_FLOW", \
	6, \
lm's avatar
lm committed
23
	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_t, time_usec) }, \
24 25 26 27 28
         { "flow_x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_optical_flow_t, flow_x) }, \
         { "flow_y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_optical_flow_t, flow_y) }, \
         { "ground_distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_optical_flow_t, ground_distance) }, \
         { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_optical_flow_t, sensor_id) }, \
         { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_optical_flow_t, quality) }, \
lm's avatar
lm committed
29 30 31 32
         } \
}


33 34 35 36 37 38
/**
 * @brief Pack a optical_flow message
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param msg The MAVLink message to compress the data into
 *
39
 * @param time_usec Timestamp (UNIX)
40
 * @param sensor_id Sensor ID
41 42
 * @param flow_x Flow in x-sensor direction
 * @param flow_y Flow in y-sensor direction
43
 * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
44
 * @param ground_distance Ground distance in meters (-1 for unknown, flow is then in pixel / arbitrary scale)
45 46
 * @return length of the message in bytes (excluding serial stream start sign)
 */
lm's avatar
lm committed
47
static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
48
						       uint64_t time_usec, uint8_t sensor_id, float flow_x, float flow_y, uint8_t quality, float ground_distance)
49
{
LM's avatar
LM committed
50
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
51
	char buf[22];
LM's avatar
LM committed
52
	_mav_put_uint64_t(buf, 0, time_usec);
53 54 55 56 57
	_mav_put_float(buf, 8, flow_x);
	_mav_put_float(buf, 12, flow_y);
	_mav_put_float(buf, 16, ground_distance);
	_mav_put_uint8_t(buf, 20, sensor_id);
	_mav_put_uint8_t(buf, 21, quality);
LM's avatar
LM committed
58

59
        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22);
LM's avatar
LM committed
60 61 62 63 64
#else
	mavlink_optical_flow_t packet;
	packet.time_usec = time_usec;
	packet.flow_x = flow_x;
	packet.flow_y = flow_y;
65
	packet.ground_distance = ground_distance;
LM's avatar
LM committed
66 67 68
	packet.sensor_id = sensor_id;
	packet.quality = quality;

69
        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22);
LM's avatar
LM committed
70
#endif
71

LM's avatar
LM committed
72
	msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW;
73
	return mavlink_finalize_message(msg, system_id, component_id, 22, 31);
74 75 76
}

/**
lm's avatar
lm committed
77
 * @brief Pack a optical_flow message on a channel
78 79 80 81
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param chan The MAVLink channel this message was sent over
 * @param msg The MAVLink message to compress the data into
82
 * @param time_usec Timestamp (UNIX)
83
 * @param sensor_id Sensor ID
84 85
 * @param flow_x Flow in x-sensor direction
 * @param flow_y Flow in y-sensor direction
86
 * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
87
 * @param ground_distance Ground distance in meters (-1 for unknown, flow is then in pixel / arbitrary scale)
88 89
 * @return length of the message in bytes (excluding serial stream start sign)
 */
lm's avatar
lm committed
90 91
static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
							   mavlink_message_t* msg,
92
						           uint64_t time_usec,uint8_t sensor_id,float flow_x,float flow_y,uint8_t quality,float ground_distance)
lm's avatar
lm committed
93
{
LM's avatar
LM committed
94
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
95
	char buf[22];
LM's avatar
LM committed
96
	_mav_put_uint64_t(buf, 0, time_usec);
97 98 99 100 101
	_mav_put_float(buf, 8, flow_x);
	_mav_put_float(buf, 12, flow_y);
	_mav_put_float(buf, 16, ground_distance);
	_mav_put_uint8_t(buf, 20, sensor_id);
	_mav_put_uint8_t(buf, 21, quality);
LM's avatar
LM committed
102

103
        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22);
LM's avatar
LM committed
104 105 106 107 108
#else
	mavlink_optical_flow_t packet;
	packet.time_usec = time_usec;
	packet.flow_x = flow_x;
	packet.flow_y = flow_y;
109
	packet.ground_distance = ground_distance;
LM's avatar
LM committed
110 111 112
	packet.sensor_id = sensor_id;
	packet.quality = quality;

113
        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22);
LM's avatar
LM committed
114
#endif
lm's avatar
lm committed
115

LM's avatar
LM committed
116
	msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW;
117
	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 22, 31);
lm's avatar
lm committed
118 119
}

120 121 122 123 124 125 126 127 128 129
/**
 * @brief Encode a optical_flow struct into a message
 *
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param msg The MAVLink message to compress the data into
 * @param optical_flow C-struct to read the message contents from
 */
static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow)
{
130
	return mavlink_msg_optical_flow_pack(system_id, component_id, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->quality, optical_flow->ground_distance);
131 132 133 134 135 136
}

/**
 * @brief Send a optical_flow message
 * @param chan MAVLink channel to send the message
 *
137
 * @param time_usec Timestamp (UNIX)
138
 * @param sensor_id Sensor ID
139 140
 * @param flow_x Flow in x-sensor direction
 * @param flow_y Flow in y-sensor direction
141
 * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
142
 * @param ground_distance Ground distance in meters (-1 for unknown, flow is then in pixel / arbitrary scale)
143
 */
lm's avatar
lm committed
144 145
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

146
static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, float flow_x, float flow_y, uint8_t quality, float ground_distance)
147
{
LM's avatar
LM committed
148
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
149
	char buf[22];
LM's avatar
LM committed
150
	_mav_put_uint64_t(buf, 0, time_usec);
151 152 153 154 155
	_mav_put_float(buf, 8, flow_x);
	_mav_put_float(buf, 12, flow_y);
	_mav_put_float(buf, 16, ground_distance);
	_mav_put_uint8_t(buf, 20, sensor_id);
	_mav_put_uint8_t(buf, 21, quality);
LM's avatar
LM committed
156

157
	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, 22, 31);
LM's avatar
LM committed
158 159 160 161 162
#else
	mavlink_optical_flow_t packet;
	packet.time_usec = time_usec;
	packet.flow_x = flow_x;
	packet.flow_y = flow_y;
163
	packet.ground_distance = ground_distance;
LM's avatar
LM committed
164 165 166
	packet.sensor_id = sensor_id;
	packet.quality = quality;

167
	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)&packet, 22, 31);
LM's avatar
LM committed
168
#endif
169 170 171
}

#endif
lm's avatar
lm committed
172

173 174
// MESSAGE OPTICAL_FLOW UNPACKING

lm's avatar
lm committed
175

176
/**
177
 * @brief Get field time_usec from optical_flow message
178 179 180
 *
 * @return Timestamp (UNIX)
 */
181
static inline uint64_t mavlink_msg_optical_flow_get_time_usec(const mavlink_message_t* msg)
182
{
LM's avatar
LM committed
183
	return _MAV_RETURN_uint64_t(msg,  0);
184 185 186 187 188 189 190 191 192
}

/**
 * @brief Get field sensor_id from optical_flow message
 *
 * @return Sensor ID
 */
static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_message_t* msg)
{
193
	return _MAV_RETURN_uint8_t(msg,  20);
194 195 196 197 198
}

/**
 * @brief Get field flow_x from optical_flow message
 *
199
 * @return Flow in x-sensor direction
200
 */
201
static inline float mavlink_msg_optical_flow_get_flow_x(const mavlink_message_t* msg)
202
{
203
	return _MAV_RETURN_float(msg,  8);
204 205 206 207 208
}

/**
 * @brief Get field flow_y from optical_flow message
 *
209
 * @return Flow in y-sensor direction
210
 */
211
static inline float mavlink_msg_optical_flow_get_flow_y(const mavlink_message_t* msg)
212
{
213
	return _MAV_RETURN_float(msg,  12);
214 215 216 217 218 219 220 221 222
}

/**
 * @brief Get field quality from optical_flow message
 *
 * @return Optical flow quality / confidence. 0: bad, 255: maximum quality
 */
static inline uint8_t mavlink_msg_optical_flow_get_quality(const mavlink_message_t* msg)
{
223
	return _MAV_RETURN_uint8_t(msg,  21);
224 225 226 227 228
}

/**
 * @brief Get field ground_distance from optical_flow message
 *
229
 * @return Ground distance in meters (-1 for unknown, flow is then in pixel / arbitrary scale)
230 231 232
 */
static inline float mavlink_msg_optical_flow_get_ground_distance(const mavlink_message_t* msg)
{
233
	return _MAV_RETURN_float(msg,  16);
234 235 236 237 238 239 240 241 242 243
}

/**
 * @brief Decode a optical_flow message into a struct
 *
 * @param msg The message to decode
 * @param optical_flow C-struct to decode the message contents into
 */
static inline void mavlink_msg_optical_flow_decode(const mavlink_message_t* msg, mavlink_optical_flow_t* optical_flow)
{
lm's avatar
lm committed
244
#if MAVLINK_NEED_BYTE_SWAP
245
	optical_flow->time_usec = mavlink_msg_optical_flow_get_time_usec(msg);
lm's avatar
lm committed
246 247
	optical_flow->flow_x = mavlink_msg_optical_flow_get_flow_x(msg);
	optical_flow->flow_y = mavlink_msg_optical_flow_get_flow_y(msg);
248
	optical_flow->ground_distance = mavlink_msg_optical_flow_get_ground_distance(msg);
lm's avatar
lm committed
249 250 251
	optical_flow->sensor_id = mavlink_msg_optical_flow_get_sensor_id(msg);
	optical_flow->quality = mavlink_msg_optical_flow_get_quality(msg);
#else
252
	memcpy(optical_flow, _MAV_PAYLOAD(msg), 22);
lm's avatar
lm committed
253
#endif
254
}