mavlink_msg_optical_flow.h 9.11 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
{
lm's avatar
lm committed
7 8 9 10 11 12
 uint64_t time; ///< Timestamp (UNIX)
 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 23 24 25 26 27 28 29 30 31 32
#define MAVLINK_MSG_ID_OPTICAL_FLOW_LEN 18
#define MAVLINK_MSG_ID_100_LEN 18



#define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW { \
	"OPTICAL_FLOW", \
	6, \
	{  { "time", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_t, time) }, \
         { "ground_distance", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_optical_flow_t, ground_distance) }, \
         { "flow_x", MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_optical_flow_t, flow_x) }, \
         { "flow_y", MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_optical_flow_t, flow_y) }, \
         { "sensor_id", MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_optical_flow_t, sensor_id) }, \
         { "quality", MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_optical_flow_t, quality) }, \
         } \
}


33 34 35 36 37 38 39 40 41 42 43 44 45 46
/**
 * @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
 *
 * @param time Timestamp (UNIX)
 * @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 48
static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
						       uint64_t time, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance)
49 50 51
{
	msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW;

lm's avatar
lm committed
52 53 54 55 56 57
	put_uint64_t_by_index(msg, 0, time); // Timestamp (UNIX)
	put_float_by_index(msg, 8, ground_distance); // Ground distance in meters
	put_int16_t_by_index(msg, 12, flow_x); // Flow in pixels in x-sensor direction
	put_int16_t_by_index(msg, 14, flow_y); // Flow in pixels in y-sensor direction
	put_uint8_t_by_index(msg, 16, sensor_id); // Sensor ID
	put_uint8_t_by_index(msg, 17, quality); // Optical flow quality / confidence. 0: bad, 255: maximum quality
58

lm's avatar
lm committed
59
	return mavlink_finalize_message(msg, system_id, component_id, 18, 146);
60 61 62
}

/**
lm's avatar
lm committed
63
 * @brief Pack a optical_flow message on a channel
64 65 66 67 68 69 70 71 72 73 74 75
 * @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
 * @param time Timestamp (UNIX)
 * @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
76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107
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,
						           uint64_t time,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,uint8_t quality,float ground_distance)
{
	msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW;

	put_uint64_t_by_index(msg, 0, time); // Timestamp (UNIX)
	put_float_by_index(msg, 8, ground_distance); // Ground distance in meters
	put_int16_t_by_index(msg, 12, flow_x); // Flow in pixels in x-sensor direction
	put_int16_t_by_index(msg, 14, flow_y); // Flow in pixels in y-sensor direction
	put_uint8_t_by_index(msg, 16, sensor_id); // Sensor ID
	put_uint8_t_by_index(msg, 17, quality); // Optical flow quality / confidence. 0: bad, 255: maximum quality

	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 146);
}

#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

/**
 * @brief Pack a optical_flow message on a channel and send
 * @param chan The MAVLink channel this message was sent over
 * @param msg The MAVLink message to compress the data into
 * @param time Timestamp (UNIX)
 * @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
 */
static inline void mavlink_msg_optical_flow_pack_chan_send(mavlink_channel_t chan,
							   mavlink_message_t* msg,
						           uint64_t time,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,uint8_t quality,float ground_distance)
108 109 110
{
	msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW;

lm's avatar
lm committed
111 112 113 114 115 116
	put_uint64_t_by_index(msg, 0, time); // Timestamp (UNIX)
	put_float_by_index(msg, 8, ground_distance); // Ground distance in meters
	put_int16_t_by_index(msg, 12, flow_x); // Flow in pixels in x-sensor direction
	put_int16_t_by_index(msg, 14, flow_y); // Flow in pixels in y-sensor direction
	put_uint8_t_by_index(msg, 16, sensor_id); // Sensor ID
	put_uint8_t_by_index(msg, 17, quality); // Optical flow quality / confidence. 0: bad, 255: maximum quality
117

lm's avatar
lm committed
118
	mavlink_finalize_message_chan_send(msg, chan, 18, 146);
119
}
lm's avatar
lm committed
120 121
#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS

122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146

/**
 * @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)
{
	return mavlink_msg_optical_flow_pack(system_id, component_id, msg, optical_flow->time, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->quality, optical_flow->ground_distance);
}

/**
 * @brief Send a optical_flow message
 * @param chan MAVLink channel to send the message
 *
 * @param time Timestamp (UNIX)
 * @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
147 148
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

149 150
static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance)
{
lm's avatar
lm committed
151 152
	MAVLINK_ALIGNED_MESSAGE(msg, 18);
	mavlink_msg_optical_flow_pack_chan_send(chan, msg, time, sensor_id, flow_x, flow_y, quality, ground_distance);
153 154 155
}

#endif
lm's avatar
lm committed
156

157 158
// MESSAGE OPTICAL_FLOW UNPACKING

lm's avatar
lm committed
159

160 161 162 163 164 165 166
/**
 * @brief Get field time from optical_flow message
 *
 * @return Timestamp (UNIX)
 */
static inline uint64_t mavlink_msg_optical_flow_get_time(const mavlink_message_t* msg)
{
lm's avatar
lm committed
167
	return MAVLINK_MSG_RETURN_uint64_t(msg,  0);
168 169 170 171 172 173 174 175 176
}

/**
 * @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
177
	return MAVLINK_MSG_RETURN_uint8_t(msg,  16);
178 179 180 181 182 183 184 185 186
}

/**
 * @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
187
	return MAVLINK_MSG_RETURN_int16_t(msg,  12);
188 189 190 191 192 193 194 195 196
}

/**
 * @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
197
	return MAVLINK_MSG_RETURN_int16_t(msg,  14);
198 199 200 201 202 203 204 205 206
}

/**
 * @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
207
	return MAVLINK_MSG_RETURN_uint8_t(msg,  17);
208 209 210 211 212 213 214 215 216
}

/**
 * @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
217
	return MAVLINK_MSG_RETURN_float(msg,  8);
218 219 220 221 222 223 224 225 226 227
}

/**
 * @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
228 229 230 231 232 233 234 235 236 237
#if MAVLINK_NEED_BYTE_SWAP
	optical_flow->time = mavlink_msg_optical_flow_get_time(msg);
	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
	memcpy(optical_flow, MAVLINK_PAYLOAD(msg), 18);
#endif
238
}