mavlink_msg_optical_flow.h 8.69 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)
lm's avatar
lm committed
8 9 10 11 12
 float ground_distance; ///< Ground distance in meters
 int16_t flow_x; ///< Flow in pixels in x-sensor direction
 int16_t flow_y; ///< Flow in pixels in y-sensor direction
 uint8_t sensor_id; ///< Sensor ID
 uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality
13 14
} mavlink_optical_flow_t;

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



#define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW { \
	"OPTICAL_FLOW", \
	6, \
lm's avatar
lm committed
23 24 25 26 27 28
	{  { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_t, time_usec) }, \
         { "ground_distance", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_optical_flow_t, ground_distance) }, \
         { "flow_x", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_optical_flow_t, flow_x) }, \
         { "flow_y", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_optical_flow_t, flow_y) }, \
         { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_optical_flow_t, sensor_id) }, \
         { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, 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 41 42 43 44 45 46
 * @param sensor_id Sensor ID
 * @param flow_x Flow in pixels in x-sensor direction
 * @param flow_y Flow in pixels in y-sensor direction
 * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
 * @param ground_distance Ground distance in meters
 * @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, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance)
49
{
LM's avatar
LM committed
50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
	char buf[18];
	_mav_put_uint64_t(buf, 0, time_usec);
	_mav_put_float(buf, 8, ground_distance);
	_mav_put_int16_t(buf, 12, flow_x);
	_mav_put_int16_t(buf, 14, flow_y);
	_mav_put_uint8_t(buf, 16, sensor_id);
	_mav_put_uint8_t(buf, 17, quality);

        memcpy(_MAV_PAYLOAD(msg), buf, 18);
#else
	mavlink_optical_flow_t packet;
	packet.time_usec = time_usec;
	packet.ground_distance = ground_distance;
	packet.flow_x = flow_x;
	packet.flow_y = flow_y;
	packet.sensor_id = sensor_id;
	packet.quality = quality;

        memcpy(_MAV_PAYLOAD(msg), &packet, 18);
#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, 18, 19);
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 84 85 86 87 88 89
 * @param sensor_id Sensor ID
 * @param flow_x Flow in pixels in x-sensor direction
 * @param flow_y Flow in pixels in y-sensor direction
 * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
 * @param ground_distance Ground distance in meters
 * @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,int16_t flow_x,int16_t flow_y,uint8_t quality,float ground_distance)
lm's avatar
lm committed
93
{
LM's avatar
LM committed
94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
	char buf[18];
	_mav_put_uint64_t(buf, 0, time_usec);
	_mav_put_float(buf, 8, ground_distance);
	_mav_put_int16_t(buf, 12, flow_x);
	_mav_put_int16_t(buf, 14, flow_y);
	_mav_put_uint8_t(buf, 16, sensor_id);
	_mav_put_uint8_t(buf, 17, quality);

        memcpy(_MAV_PAYLOAD(msg), buf, 18);
#else
	mavlink_optical_flow_t packet;
	packet.time_usec = time_usec;
	packet.ground_distance = ground_distance;
	packet.flow_x = flow_x;
	packet.flow_y = flow_y;
	packet.sensor_id = sensor_id;
	packet.quality = quality;

        memcpy(_MAV_PAYLOAD(msg), &packet, 18);
#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, 18, 19);
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 139 140 141 142 143
 * @param sensor_id Sensor ID
 * @param flow_x Flow in pixels in x-sensor direction
 * @param flow_y Flow in pixels in y-sensor direction
 * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
 * @param ground_distance Ground distance in meters
 */
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, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance)
147
{
LM's avatar
LM committed
148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
	char buf[18];
	_mav_put_uint64_t(buf, 0, time_usec);
	_mav_put_float(buf, 8, ground_distance);
	_mav_put_int16_t(buf, 12, flow_x);
	_mav_put_int16_t(buf, 14, flow_y);
	_mav_put_uint8_t(buf, 16, sensor_id);
	_mav_put_uint8_t(buf, 17, quality);

	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, 18, 19);
#else
	mavlink_optical_flow_t packet;
	packet.time_usec = time_usec;
	packet.ground_distance = ground_distance;
	packet.flow_x = flow_x;
	packet.flow_y = flow_y;
	packet.sensor_id = sensor_id;
	packet.quality = quality;

	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)&packet, 18, 19);
#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)
{
LM's avatar
LM committed
193
	return _MAV_RETURN_uint8_t(msg,  16);
194 195 196 197 198 199 200 201 202
}

/**
 * @brief Get field flow_x from optical_flow message
 *
 * @return Flow in pixels in x-sensor direction
 */
static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_t* msg)
{
LM's avatar
LM committed
203
	return _MAV_RETURN_int16_t(msg,  12);
204 205 206 207 208 209 210 211 212
}

/**
 * @brief Get field flow_y from optical_flow message
 *
 * @return Flow in pixels in y-sensor direction
 */
static inline int16_t mavlink_msg_optical_flow_get_flow_y(const mavlink_message_t* msg)
{
LM's avatar
LM committed
213
	return _MAV_RETURN_int16_t(msg,  14);
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)
{
LM's avatar
LM committed
223
	return _MAV_RETURN_uint8_t(msg,  17);
224 225 226 227 228 229 230 231 232
}

/**
 * @brief Get field ground_distance from optical_flow message
 *
 * @return Ground distance in meters
 */
static inline float mavlink_msg_optical_flow_get_ground_distance(const mavlink_message_t* msg)
{
LM's avatar
LM committed
233
	return _MAV_RETURN_float(msg,  8);
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 248 249 250 251
	optical_flow->ground_distance = mavlink_msg_optical_flow_get_ground_distance(msg);
	optical_flow->flow_x = mavlink_msg_optical_flow_get_flow_x(msg);
	optical_flow->flow_y = mavlink_msg_optical_flow_get_flow_y(msg);
	optical_flow->sensor_id = mavlink_msg_optical_flow_get_sensor_id(msg);
	optical_flow->quality = mavlink_msg_optical_flow_get_quality(msg);
#else
LM's avatar
LM committed
252
	memcpy(optical_flow, _MAV_PAYLOAD(msg), 18);
lm's avatar
lm committed
253
#endif
254
}