mavlink_msg_hil_state.h 15 KB
Newer Older
1
// MESSAGE HIL_STATE PACKING
2

3 4 5 6 7
#define MAVLINK_MSG_ID_HIL_STATE 90
#define MAVLINK_MSG_ID_HIL_STATE_LEN 56
#define MAVLINK_MSG_90_LEN 56
#define MAVLINK_MSG_ID_HIL_STATE_KEY 0x12
#define MAVLINK_MSG_90_KEY 0x12
8

9
typedef struct __mavlink_hil_state_t 
10
{
11
	uint64_t time_us;	///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
lm's avatar
lm committed
12 13 14 15 16 17 18 19 20 21 22 23 24 25 26
	float roll;	///< Roll angle (rad)
	float pitch;	///< Pitch angle (rad)
	float yaw;	///< Yaw angle (rad)
	float rollspeed;	///< Roll angular speed (rad/s)
	float pitchspeed;	///< Pitch angular speed (rad/s)
	float yawspeed;	///< Yaw angular speed (rad/s)
	int32_t lat;	///< Latitude, expressed as * 1E7
	int32_t lon;	///< Longitude, expressed as * 1E7
	int32_t alt;	///< Altitude in meters, expressed as * 1000 (millimeters)
	int16_t vx;	///< Ground X Speed (Latitude), expressed as m/s * 100
	int16_t vy;	///< Ground Y Speed (Longitude), expressed as m/s * 100
	int16_t vz;	///< Ground Z Speed (Altitude), expressed as m/s * 100
	int16_t xacc;	///< X acceleration (mg)
	int16_t yacc;	///< Y acceleration (mg)
	int16_t zacc;	///< Z acceleration (mg)
27

28
} mavlink_hil_state_t;
29 30

/**
31
 * @brief Pack a hil_state message
32 33 34 35
 * @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
 *
36
 * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot)
37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53
 * @param roll Roll angle (rad)
 * @param pitch Pitch angle (rad)
 * @param yaw Yaw angle (rad)
 * @param rollspeed Roll angular speed (rad/s)
 * @param pitchspeed Pitch angular speed (rad/s)
 * @param yawspeed Yaw angular speed (rad/s)
 * @param lat Latitude, expressed as * 1E7
 * @param lon Longitude, expressed as * 1E7
 * @param alt Altitude in meters, expressed as * 1000 (millimeters)
 * @param vx Ground X Speed (Latitude), expressed as m/s * 100
 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
 * @param xacc X acceleration (mg)
 * @param yacc Y acceleration (mg)
 * @param zacc Z acceleration (mg)
 * @return length of the message in bytes (excluding serial stream start sign)
 */
54
static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_us, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
55
{
56 57
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
	msg->msgid = MAVLINK_MSG_ID_HIL_STATE;
58

59
	p->time_us = time_us;	// uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot)
lm's avatar
lm committed
60 61 62 63 64 65 66 67 68 69 70 71 72 73 74
	p->roll = roll;	// float:Roll angle (rad)
	p->pitch = pitch;	// float:Pitch angle (rad)
	p->yaw = yaw;	// float:Yaw angle (rad)
	p->rollspeed = rollspeed;	// float:Roll angular speed (rad/s)
	p->pitchspeed = pitchspeed;	// float:Pitch angular speed (rad/s)
	p->yawspeed = yawspeed;	// float:Yaw angular speed (rad/s)
	p->lat = lat;	// int32_t:Latitude, expressed as * 1E7
	p->lon = lon;	// int32_t:Longitude, expressed as * 1E7
	p->alt = alt;	// int32_t:Altitude in meters, expressed as * 1000 (millimeters)
	p->vx = vx;	// int16_t:Ground X Speed (Latitude), expressed as m/s * 100
	p->vy = vy;	// int16_t:Ground Y Speed (Longitude), expressed as m/s * 100
	p->vz = vz;	// int16_t:Ground Z Speed (Altitude), expressed as m/s * 100
	p->xacc = xacc;	// int16_t:X acceleration (mg)
	p->yacc = yacc;	// int16_t:Y acceleration (mg)
	p->zacc = zacc;	// int16_t:Z acceleration (mg)
75

76
	return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_LEN);
77 78 79
}

/**
80
 * @brief Pack a hil_state message
81 82 83 84
 * @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
85
 * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot)
86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102
 * @param roll Roll angle (rad)
 * @param pitch Pitch angle (rad)
 * @param yaw Yaw angle (rad)
 * @param rollspeed Roll angular speed (rad/s)
 * @param pitchspeed Pitch angular speed (rad/s)
 * @param yawspeed Yaw angular speed (rad/s)
 * @param lat Latitude, expressed as * 1E7
 * @param lon Longitude, expressed as * 1E7
 * @param alt Altitude in meters, expressed as * 1000 (millimeters)
 * @param vx Ground X Speed (Latitude), expressed as m/s * 100
 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
 * @param xacc X acceleration (mg)
 * @param yacc Y acceleration (mg)
 * @param zacc Z acceleration (mg)
 * @return length of the message in bytes (excluding serial stream start sign)
 */
103
static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time_us, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
104
{
105 106
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
	msg->msgid = MAVLINK_MSG_ID_HIL_STATE;
107

108
	p->time_us = time_us;	// uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot)
lm's avatar
lm committed
109 110 111 112 113 114 115 116 117 118 119 120 121 122 123
	p->roll = roll;	// float:Roll angle (rad)
	p->pitch = pitch;	// float:Pitch angle (rad)
	p->yaw = yaw;	// float:Yaw angle (rad)
	p->rollspeed = rollspeed;	// float:Roll angular speed (rad/s)
	p->pitchspeed = pitchspeed;	// float:Pitch angular speed (rad/s)
	p->yawspeed = yawspeed;	// float:Yaw angular speed (rad/s)
	p->lat = lat;	// int32_t:Latitude, expressed as * 1E7
	p->lon = lon;	// int32_t:Longitude, expressed as * 1E7
	p->alt = alt;	// int32_t:Altitude in meters, expressed as * 1000 (millimeters)
	p->vx = vx;	// int16_t:Ground X Speed (Latitude), expressed as m/s * 100
	p->vy = vy;	// int16_t:Ground Y Speed (Longitude), expressed as m/s * 100
	p->vz = vz;	// int16_t:Ground Z Speed (Altitude), expressed as m/s * 100
	p->xacc = xacc;	// int16_t:X acceleration (mg)
	p->yacc = yacc;	// int16_t:Y acceleration (mg)
	p->zacc = zacc;	// int16_t:Z acceleration (mg)
124

125
	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_LEN);
126 127 128
}

/**
129
 * @brief Encode a hil_state struct into a message
130 131 132 133
 *
 * @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
134
 * @param hil_state C-struct to read the message contents from
135
 */
136
static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state)
137
{
138
	return mavlink_msg_hil_state_pack(system_id, component_id, msg, hil_state->time_us, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc);
139 140
}

lm's avatar
lm committed
141 142

#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
143
/**
144
 * @brief Send a hil_state message
145 146
 * @param chan MAVLink channel to send the message
 *
147
 * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot)
148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163
 * @param roll Roll angle (rad)
 * @param pitch Pitch angle (rad)
 * @param yaw Yaw angle (rad)
 * @param rollspeed Roll angular speed (rad/s)
 * @param pitchspeed Pitch angular speed (rad/s)
 * @param yawspeed Yaw angular speed (rad/s)
 * @param lat Latitude, expressed as * 1E7
 * @param lon Longitude, expressed as * 1E7
 * @param alt Altitude in meters, expressed as * 1000 (millimeters)
 * @param vx Ground X Speed (Latitude), expressed as m/s * 100
 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
 * @param xacc X acceleration (mg)
 * @param yacc Y acceleration (mg)
 * @param zacc Z acceleration (mg)
 */
164
static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t time_us, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
165 166
{
	mavlink_header_t hdr;
167
	mavlink_hil_state_t payload;
lm's avatar
lm committed
168

169 170
	MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_HIL_STATE_LEN )
	payload.time_us = time_us;	// uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot)
lm's avatar
lm committed
171 172 173 174 175 176 177 178 179 180 181 182 183 184 185
	payload.roll = roll;	// float:Roll angle (rad)
	payload.pitch = pitch;	// float:Pitch angle (rad)
	payload.yaw = yaw;	// float:Yaw angle (rad)
	payload.rollspeed = rollspeed;	// float:Roll angular speed (rad/s)
	payload.pitchspeed = pitchspeed;	// float:Pitch angular speed (rad/s)
	payload.yawspeed = yawspeed;	// float:Yaw angular speed (rad/s)
	payload.lat = lat;	// int32_t:Latitude, expressed as * 1E7
	payload.lon = lon;	// int32_t:Longitude, expressed as * 1E7
	payload.alt = alt;	// int32_t:Altitude in meters, expressed as * 1000 (millimeters)
	payload.vx = vx;	// int16_t:Ground X Speed (Latitude), expressed as m/s * 100
	payload.vy = vy;	// int16_t:Ground Y Speed (Longitude), expressed as m/s * 100
	payload.vz = vz;	// int16_t:Ground Z Speed (Altitude), expressed as m/s * 100
	payload.xacc = xacc;	// int16_t:X acceleration (mg)
	payload.yacc = yacc;	// int16_t:Y acceleration (mg)
	payload.zacc = zacc;	// int16_t:Z acceleration (mg)
186 187

	hdr.STX = MAVLINK_STX;
188 189
	hdr.len = MAVLINK_MSG_ID_HIL_STATE_LEN;
	hdr.msgid = MAVLINK_MSG_ID_HIL_STATE;
190 191 192 193 194
	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 );
195
	mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) );
196

lm's avatar
lm committed
197 198 199
	crc_init(&hdr.ck);
	crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN);
	crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len );
200
	crc_accumulate( 0x12, &hdr.ck); /// include key in X25 checksum
lm's avatar
lm committed
201 202
	mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES);
	MAVLINK_BUFFER_CHECK_END
203 204 205
}

#endif
206
// MESSAGE HIL_STATE UNPACKING
207 208

/**
209
 * @brief Get field time_us from hil_state message
210 211 212
 *
 * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
 */
213
static inline uint64_t mavlink_msg_hil_state_get_time_us(const mavlink_message_t* msg)
214
{
215 216
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
	return (uint64_t)(p->time_us);
217 218 219
}

/**
220
 * @brief Get field roll from hil_state message
221 222 223
 *
 * @return Roll angle (rad)
 */
224
static inline float mavlink_msg_hil_state_get_roll(const mavlink_message_t* msg)
225
{
226
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
227 228 229 230
	return (float)(p->roll);
}

/**
231
 * @brief Get field pitch from hil_state message
232 233 234
 *
 * @return Pitch angle (rad)
 */
235
static inline float mavlink_msg_hil_state_get_pitch(const mavlink_message_t* msg)
236
{
237
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
238 239 240 241
	return (float)(p->pitch);
}

/**
242
 * @brief Get field yaw from hil_state message
243 244 245
 *
 * @return Yaw angle (rad)
 */
246
static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg)
247
{
248
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
249 250 251 252
	return (float)(p->yaw);
}

/**
253
 * @brief Get field rollspeed from hil_state message
254 255 256
 *
 * @return Roll angular speed (rad/s)
 */
257
static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* msg)
258
{
259
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
260 261 262 263
	return (float)(p->rollspeed);
}

/**
264
 * @brief Get field pitchspeed from hil_state message
265 266 267
 *
 * @return Pitch angular speed (rad/s)
 */
268
static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t* msg)
269
{
270
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
271 272 273 274
	return (float)(p->pitchspeed);
}

/**
275
 * @brief Get field yawspeed from hil_state message
276 277 278
 *
 * @return Yaw angular speed (rad/s)
 */
279
static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* msg)
280
{
281
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
282 283 284 285
	return (float)(p->yawspeed);
}

/**
286
 * @brief Get field lat from hil_state message
287 288 289
 *
 * @return Latitude, expressed as * 1E7
 */
290
static inline int32_t mavlink_msg_hil_state_get_lat(const mavlink_message_t* msg)
291
{
292
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
293 294 295 296
	return (int32_t)(p->lat);
}

/**
297
 * @brief Get field lon from hil_state message
298 299 300
 *
 * @return Longitude, expressed as * 1E7
 */
301
static inline int32_t mavlink_msg_hil_state_get_lon(const mavlink_message_t* msg)
302
{
303
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
304 305 306 307
	return (int32_t)(p->lon);
}

/**
308
 * @brief Get field alt from hil_state message
309 310 311
 *
 * @return Altitude in meters, expressed as * 1000 (millimeters)
 */
312
static inline int32_t mavlink_msg_hil_state_get_alt(const mavlink_message_t* msg)
313
{
314
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
315 316 317 318
	return (int32_t)(p->alt);
}

/**
319
 * @brief Get field vx from hil_state message
320 321 322
 *
 * @return Ground X Speed (Latitude), expressed as m/s * 100
 */
323
static inline int16_t mavlink_msg_hil_state_get_vx(const mavlink_message_t* msg)
324
{
325
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
326 327 328 329
	return (int16_t)(p->vx);
}

/**
330
 * @brief Get field vy from hil_state message
331 332 333
 *
 * @return Ground Y Speed (Longitude), expressed as m/s * 100
 */
334
static inline int16_t mavlink_msg_hil_state_get_vy(const mavlink_message_t* msg)
335
{
336
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
337 338 339 340
	return (int16_t)(p->vy);
}

/**
341
 * @brief Get field vz from hil_state message
342 343 344
 *
 * @return Ground Z Speed (Altitude), expressed as m/s * 100
 */
345
static inline int16_t mavlink_msg_hil_state_get_vz(const mavlink_message_t* msg)
346
{
347
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
348 349 350 351
	return (int16_t)(p->vz);
}

/**
352
 * @brief Get field xacc from hil_state message
353 354 355
 *
 * @return X acceleration (mg)
 */
356
static inline int16_t mavlink_msg_hil_state_get_xacc(const mavlink_message_t* msg)
357
{
358
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
359 360 361 362
	return (int16_t)(p->xacc);
}

/**
363
 * @brief Get field yacc from hil_state message
364 365 366
 *
 * @return Y acceleration (mg)
 */
367
static inline int16_t mavlink_msg_hil_state_get_yacc(const mavlink_message_t* msg)
368
{
369
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
370 371 372 373
	return (int16_t)(p->yacc);
}

/**
374
 * @brief Get field zacc from hil_state message
375 376 377
 *
 * @return Z acceleration (mg)
 */
378
static inline int16_t mavlink_msg_hil_state_get_zacc(const mavlink_message_t* msg)
379
{
380
	mavlink_hil_state_t *p = (mavlink_hil_state_t *)&msg->payload[0];
381 382 383 384
	return (int16_t)(p->zacc);
}

/**
385
 * @brief Decode a hil_state message into a struct
386 387
 *
 * @param msg The message to decode
388
 * @param hil_state C-struct to decode the message contents into
389
 */
390
static inline void mavlink_msg_hil_state_decode(const mavlink_message_t* msg, mavlink_hil_state_t* hil_state)
391
{
392
	memcpy( hil_state, msg->payload, sizeof(mavlink_hil_state_t));
393
}