mavlink_msg_vision_speed_estimate.h 7.26 KB
Newer Older
LM's avatar
LM committed
1 2 3
// MESSAGE VISION_SPEED_ESTIMATE PACKING

#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE 113
lm's avatar
lm committed
4 5
#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN 20
#define MAVLINK_MSG_113_LEN 20
LM's avatar
LM committed
6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29

typedef struct __mavlink_vision_speed_estimate_t 
{
	uint64_t usec; ///< Timestamp (milliseconds)
	float x; ///< Global X speed
	float y; ///< Global Y speed
	float z; ///< Global Z speed

} mavlink_vision_speed_estimate_t;

/**
 * @brief Pack a vision_speed_estimate 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 usec Timestamp (milliseconds)
 * @param x Global X speed
 * @param y Global Y speed
 * @param z Global Z speed
 * @return length of the message in bytes (excluding serial stream start sign)
 */
static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z)
{
lm's avatar
lm committed
30
	mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0];
LM's avatar
LM committed
31 32
	msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE;

lm's avatar
lm committed
33 34 35 36
	p->usec = usec; // uint64_t:Timestamp (milliseconds)
	p->x = x; // float:Global X speed
	p->y = y; // float:Global Y speed
	p->z = z; // float:Global Z speed
LM's avatar
LM committed
37

lm's avatar
lm committed
38
	return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
LM's avatar
LM committed
39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54
}

/**
 * @brief Pack a vision_speed_estimate message
 * @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 usec Timestamp (milliseconds)
 * @param x Global X speed
 * @param y Global Y speed
 * @param z Global Z speed
 * @return length of the message in bytes (excluding serial stream start sign)
 */
static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float x, float y, float z)
{
lm's avatar
lm committed
55
	mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0];
LM's avatar
LM committed
56 57
	msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE;

lm's avatar
lm committed
58 59 60 61
	p->usec = usec; // uint64_t:Timestamp (milliseconds)
	p->x = x; // float:Global X speed
	p->y = y; // float:Global Y speed
	p->z = z; // float:Global Z speed
LM's avatar
LM committed
62

lm's avatar
lm committed
63
	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
LM's avatar
LM committed
64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91
}

/**
 * @brief Encode a vision_speed_estimate 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 vision_speed_estimate C-struct to read the message contents from
 */
static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate)
{
	return mavlink_msg_vision_speed_estimate_pack(system_id, component_id, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z);
}

/**
 * @brief Send a vision_speed_estimate message
 * @param chan MAVLink channel to send the message
 *
 * @param usec Timestamp (milliseconds)
 * @param x Global X speed
 * @param y Global Y speed
 * @param z Global Z speed
 */
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z)
{
	mavlink_message_t msg;
lm's avatar
lm committed
92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145
	uint16_t checksum;
	mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg.payload[0];

	p->usec = usec; // uint64_t:Timestamp (milliseconds)
	p->x = x; // float:Global X speed
	p->y = y; // float:Global Y speed
	p->z = z; // float:Global Z speed

	msg.STX = MAVLINK_STX;
	msg.len = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN;
	msg.msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE;
	msg.sysid = mavlink_system.sysid;
	msg.compid = mavlink_system.compid;
	msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
	mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
	checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
	msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
	msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte

	mavlink_send_msg(chan, &msg);
}

#endif

#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z)
{
	mavlink_header_t hdr;
	mavlink_vision_speed_estimate_t payload;
	uint16_t checksum;
	mavlink_vision_speed_estimate_t *p = &payload;

	p->usec = usec; // uint64_t:Timestamp (milliseconds)
	p->x = x; // float:Global X speed
	p->y = y; // float:Global Y speed
	p->z = z; // float:Global Z speed

	hdr.STX = MAVLINK_STX;
	hdr.len = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN;
	hdr.msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE;
	hdr.sysid = mavlink_system.sysid;
	hdr.compid = mavlink_system.compid;
	hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq;
	mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1;
	mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES );

	crc_init(&checksum);
	checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN);
	checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len );
	hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
	hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte

	mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len);
	mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES);
LM's avatar
LM committed
146 147 148 149 150 151 152 153 154 155 156 157
}

#endif
// MESSAGE VISION_SPEED_ESTIMATE UNPACKING

/**
 * @brief Get field usec from vision_speed_estimate message
 *
 * @return Timestamp (milliseconds)
 */
static inline uint64_t mavlink_msg_vision_speed_estimate_get_usec(const mavlink_message_t* msg)
{
lm's avatar
lm committed
158 159
	mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0];
	return (uint64_t)(p->usec);
LM's avatar
LM committed
160 161 162 163 164 165 166 167 168
}

/**
 * @brief Get field x from vision_speed_estimate message
 *
 * @return Global X speed
 */
static inline float mavlink_msg_vision_speed_estimate_get_x(const mavlink_message_t* msg)
{
lm's avatar
lm committed
169 170
	mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0];
	return (float)(p->x);
LM's avatar
LM committed
171 172 173 174 175 176 177 178 179
}

/**
 * @brief Get field y from vision_speed_estimate message
 *
 * @return Global Y speed
 */
static inline float mavlink_msg_vision_speed_estimate_get_y(const mavlink_message_t* msg)
{
lm's avatar
lm committed
180 181
	mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0];
	return (float)(p->y);
LM's avatar
LM committed
182 183 184 185 186 187 188 189 190
}

/**
 * @brief Get field z from vision_speed_estimate message
 *
 * @return Global Z speed
 */
static inline float mavlink_msg_vision_speed_estimate_get_z(const mavlink_message_t* msg)
{
lm's avatar
lm committed
191 192
	mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0];
	return (float)(p->z);
LM's avatar
LM committed
193 194 195 196 197 198 199 200 201 202
}

/**
 * @brief Decode a vision_speed_estimate message into a struct
 *
 * @param msg The message to decode
 * @param vision_speed_estimate C-struct to decode the message contents into
 */
static inline void mavlink_msg_vision_speed_estimate_decode(const mavlink_message_t* msg, mavlink_vision_speed_estimate_t* vision_speed_estimate)
{
lm's avatar
lm committed
203
	memcpy( vision_speed_estimate, msg->payload, sizeof(mavlink_vision_speed_estimate_t));
LM's avatar
LM committed
204
}