mavlink_message_attitude.h 1.34 KB
Newer Older
pixhawk's avatar
pixhawk committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56
// MESSAGE ATTITUDE PACKING

#define MESSAGE_ID_ATTITUDE 90

/**
 * @brief Send a attitude message
 *
 * @param roll Roll angle (rad)
 * @param pitch Pitch angle (rad)
 * @param yaw Yaw angle (rad)
 * @return length of the message in bytes (excluding serial stream start sign)
 */
static inline uint16_t message_attitude_pack(uint8_t system_id, CommMessage_t* msg, float roll, float pitch, float yaw)
{
	msg->msgid = MESSAGE_ID_ATTITUDE;
	uint16_t i = 0;

	i += put_float_by_index(roll, i, msg->payload); //Roll angle (rad)
	i += put_float_by_index(pitch, i, msg->payload); //Pitch angle (rad)
	i += put_float_by_index(yaw, i, msg->payload); //Yaw angle (rad)

	return finalize_message(msg, system_id, i);
}

// MESSAGE ATTITUDE UNPACKING

/**
 * @brief Get field roll from attitude message
 *
 * @return Roll angle (rad)
 */
static inline float message_attitude_get_roll(CommMessage_t* msg)
{
	return *((float*) (void*)msg->payload);
}

/**
 * @brief Get field pitch from attitude message
 *
 * @return Pitch angle (rad)
 */
static inline float message_attitude_get_pitch(CommMessage_t* msg)
{
	return *((float*) (void*)msg->payload+sizeof(float));
}

/**
 * @brief Get field yaw from attitude message
 *
 * @return Yaw angle (rad)
 */
static inline float message_attitude_get_yaw(CommMessage_t* msg)
{
	return *((float*) (void*)msg->payload+sizeof(float)+sizeof(float));
}