mavlink_msg_optical_flow.h 8.28 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, \
23
	{  { "time_usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_t, time_usec) }, \
lm's avatar
lm committed
24 25 26 27 28 29 30 31 32
         { "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
/**
 * @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 50 51
{
	msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW;

52
	put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (UNIX)
lm's avatar
lm committed
53 54 55 56 57
	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

59
	return mavlink_finalize_message(msg, system_id, component_id, 18, 19);
60 61 62
}

/**
lm's avatar
lm committed
63
 * @brief Pack a optical_flow message on a channel
64 65 66 67
 * @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
68
 * @param time_usec Timestamp (UNIX)
69 70 71 72 73 74 75
 * @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
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,
78
						           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
79 80 81
{
	msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW;

82
	put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (UNIX)
lm's avatar
lm committed
83 84 85 86 87 88
	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

89
	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 19);
lm's avatar
lm committed
90 91
}

92 93 94 95 96 97 98 99 100 101
/**
 * @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)
{
102
	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);
103 104 105 106 107 108
}

/**
 * @brief Send a optical_flow message
 * @param chan MAVLink channel to send the message
 *
109
 * @param time_usec Timestamp (UNIX)
110 111 112 113 114 115
 * @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
116 117
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

118
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)
119
{
lm's avatar
lm committed
120
	MAVLINK_ALIGNED_MESSAGE(msg, 18);
LM's avatar
LM committed
121 122
	msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW;

123
	put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (UNIX)
LM's avatar
LM committed
124 125 126 127 128 129
	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

130
	mavlink_finalize_message_chan_send(msg, chan, 18, 19);
131 132 133
}

#endif
lm's avatar
lm committed
134

135 136
// MESSAGE OPTICAL_FLOW UNPACKING

lm's avatar
lm committed
137

138
/**
139
 * @brief Get field time_usec from optical_flow message
140 141 142
 *
 * @return Timestamp (UNIX)
 */
143
static inline uint64_t mavlink_msg_optical_flow_get_time_usec(const mavlink_message_t* msg)
144
{
lm's avatar
lm committed
145
	return MAVLINK_MSG_RETURN_uint64_t(msg,  0);
146 147 148 149 150 151 152 153 154
}

/**
 * @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
155
	return MAVLINK_MSG_RETURN_uint8_t(msg,  16);
156 157 158 159 160 161 162 163 164
}

/**
 * @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
165
	return MAVLINK_MSG_RETURN_int16_t(msg,  12);
166 167 168 169 170 171 172 173 174
}

/**
 * @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
175
	return MAVLINK_MSG_RETURN_int16_t(msg,  14);
176 177 178 179 180 181 182 183 184
}

/**
 * @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
185
	return MAVLINK_MSG_RETURN_uint8_t(msg,  17);
186 187 188 189 190 191 192 193 194
}

/**
 * @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
195
	return MAVLINK_MSG_RETURN_float(msg,  8);
196 197 198 199 200 201 202 203 204 205
}

/**
 * @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
206
#if MAVLINK_NEED_BYTE_SWAP
207
	optical_flow->time_usec = mavlink_msg_optical_flow_get_time_usec(msg);
lm's avatar
lm committed
208 209 210 211 212 213 214 215
	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
216
}