mavlink_msg_sensor_bias.h 8.27 KB
Newer Older
James Goppert's avatar
James Goppert committed
1 2 3 4
// MESSAGE SENSOR_BIAS PACKING

#define MAVLINK_MSG_ID_SENSOR_BIAS 172

lm's avatar
lm committed
5
typedef struct __mavlink_sensor_bias_t
James Goppert's avatar
James Goppert committed
6
{
lm's avatar
lm committed
7 8 9 10 11 12
 float axBias; ///< Accelerometer X bias (m/s)
 float ayBias; ///< Accelerometer Y bias (m/s)
 float azBias; ///< Accelerometer Z bias (m/s)
 float gxBias; ///< Gyro X bias (rad/s)
 float gyBias; ///< Gyro Y bias (rad/s)
 float gzBias; ///< Gyro Z bias (rad/s)
James Goppert's avatar
James Goppert committed
13 14
} mavlink_sensor_bias_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_SENSOR_BIAS_LEN 24
#define MAVLINK_MSG_ID_172_LEN 24



#define MAVLINK_MESSAGE_INFO_SENSOR_BIAS { \
	"SENSOR_BIAS", \
	6, \
	{  { "axBias", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sensor_bias_t, axBias) }, \
         { "ayBias", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_sensor_bias_t, ayBias) }, \
         { "azBias", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_sensor_bias_t, azBias) }, \
         { "gxBias", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_sensor_bias_t, gxBias) }, \
         { "gyBias", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_sensor_bias_t, gyBias) }, \
         { "gzBias", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_sensor_bias_t, gzBias) }, \
         } \
}


James Goppert's avatar
James Goppert committed
33
/**
James Goppert's avatar
James Goppert committed
34 35 36 37
 * @brief Pack a sensor_bias 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
James Goppert's avatar
James Goppert committed
38 39 40 41 42 43 44 45 46
 *
 * @param axBias Accelerometer X bias (m/s)
 * @param ayBias Accelerometer Y bias (m/s)
 * @param azBias Accelerometer Z bias (m/s)
 * @param gxBias Gyro X bias (rad/s)
 * @param gyBias Gyro Y bias (rad/s)
 * @param gzBias Gyro Z bias (rad/s)
 * @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_sensor_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
						       float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias)
James Goppert's avatar
James Goppert committed
49 50 51
{
	msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS;

lm's avatar
lm committed
52 53 54 55 56 57
	put_float_by_index(msg, 0, axBias); // Accelerometer X bias (m/s)
	put_float_by_index(msg, 4, ayBias); // Accelerometer Y bias (m/s)
	put_float_by_index(msg, 8, azBias); // Accelerometer Z bias (m/s)
	put_float_by_index(msg, 12, gxBias); // Gyro X bias (rad/s)
	put_float_by_index(msg, 16, gyBias); // Gyro Y bias (rad/s)
	put_float_by_index(msg, 20, gzBias); // Gyro Z bias (rad/s)
James Goppert's avatar
James Goppert committed
58

lm's avatar
lm committed
59
	return mavlink_finalize_message(msg, system_id, component_id, 24, 168);
James Goppert's avatar
James Goppert committed
60 61
}

James Goppert's avatar
James Goppert committed
62
/**
lm's avatar
lm committed
63
 * @brief Pack a sensor_bias message on a channel
James Goppert's avatar
James Goppert committed
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 axBias Accelerometer X bias (m/s)
 * @param ayBias Accelerometer Y bias (m/s)
 * @param azBias Accelerometer Z bias (m/s)
 * @param gxBias Gyro X bias (rad/s)
 * @param gyBias Gyro Y bias (rad/s)
 * @param gzBias Gyro Z bias (rad/s)
 * @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_sensor_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
							   mavlink_message_t* msg,
						           float axBias,float ayBias,float azBias,float gxBias,float gyBias,float gzBias)
{
	msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS;

	put_float_by_index(msg, 0, axBias); // Accelerometer X bias (m/s)
	put_float_by_index(msg, 4, ayBias); // Accelerometer Y bias (m/s)
	put_float_by_index(msg, 8, azBias); // Accelerometer Z bias (m/s)
	put_float_by_index(msg, 12, gxBias); // Gyro X bias (rad/s)
	put_float_by_index(msg, 16, gyBias); // Gyro Y bias (rad/s)
	put_float_by_index(msg, 20, gzBias); // Gyro Z bias (rad/s)

	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 168);
}

#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

/**
 * @brief Pack a sensor_bias 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 axBias Accelerometer X bias (m/s)
 * @param ayBias Accelerometer Y bias (m/s)
 * @param azBias Accelerometer Z bias (m/s)
 * @param gxBias Gyro X bias (rad/s)
 * @param gyBias Gyro Y bias (rad/s)
 * @param gzBias Gyro Z bias (rad/s)
 */
static inline void mavlink_msg_sensor_bias_pack_chan_send(mavlink_channel_t chan,
							   mavlink_message_t* msg,
						           float axBias,float ayBias,float azBias,float gxBias,float gyBias,float gzBias)
James Goppert's avatar
James Goppert committed
108 109 110
{
	msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS;

lm's avatar
lm committed
111 112 113 114 115 116
	put_float_by_index(msg, 0, axBias); // Accelerometer X bias (m/s)
	put_float_by_index(msg, 4, ayBias); // Accelerometer Y bias (m/s)
	put_float_by_index(msg, 8, azBias); // Accelerometer Z bias (m/s)
	put_float_by_index(msg, 12, gxBias); // Gyro X bias (rad/s)
	put_float_by_index(msg, 16, gyBias); // Gyro Y bias (rad/s)
	put_float_by_index(msg, 20, gzBias); // Gyro Z bias (rad/s)
James Goppert's avatar
James Goppert committed
117

lm's avatar
lm committed
118
	mavlink_finalize_message_chan_send(msg, chan, 24, 168);
James Goppert's avatar
James Goppert committed
119
}
lm's avatar
lm committed
120 121
#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS

James Goppert's avatar
James Goppert committed
122

James Goppert's avatar
James Goppert committed
123 124 125 126 127 128 129 130
/**
 * @brief Encode a sensor_bias 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 sensor_bias C-struct to read the message contents from
 */
James Goppert's avatar
James Goppert committed
131 132 133 134 135
static inline uint16_t mavlink_msg_sensor_bias_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sensor_bias_t* sensor_bias)
{
	return mavlink_msg_sensor_bias_pack(system_id, component_id, msg, sensor_bias->axBias, sensor_bias->ayBias, sensor_bias->azBias, sensor_bias->gxBias, sensor_bias->gyBias, sensor_bias->gzBias);
}

James Goppert's avatar
James Goppert committed
136 137 138 139 140 141 142 143 144 145 146
/**
 * @brief Send a sensor_bias message
 * @param chan MAVLink channel to send the message
 *
 * @param axBias Accelerometer X bias (m/s)
 * @param ayBias Accelerometer Y bias (m/s)
 * @param azBias Accelerometer Z bias (m/s)
 * @param gxBias Gyro X bias (rad/s)
 * @param gyBias Gyro Y bias (rad/s)
 * @param gzBias Gyro Z bias (rad/s)
 */
lm's avatar
lm committed
147 148
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

lm's avatar
lm committed
149 150
static inline void mavlink_msg_sensor_bias_send(mavlink_channel_t chan, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias)
{
lm's avatar
lm committed
151 152
	MAVLINK_ALIGNED_MESSAGE(msg, 24);
	mavlink_msg_sensor_bias_pack_chan_send(chan, msg, axBias, ayBias, azBias, gxBias, gyBias, gzBias);
James Goppert's avatar
James Goppert committed
153 154 155
}

#endif
lm's avatar
lm committed
156

James Goppert's avatar
James Goppert committed
157 158
// MESSAGE SENSOR_BIAS UNPACKING

lm's avatar
lm committed
159

James Goppert's avatar
James Goppert committed
160 161 162 163 164 165 166
/**
 * @brief Get field axBias from sensor_bias message
 *
 * @return Accelerometer X bias (m/s)
 */
static inline float mavlink_msg_sensor_bias_get_axBias(const mavlink_message_t* msg)
{
lm's avatar
lm committed
167
	return MAVLINK_MSG_RETURN_float(msg,  0);
James Goppert's avatar
James Goppert committed
168 169 170 171 172 173 174 175 176
}

/**
 * @brief Get field ayBias from sensor_bias message
 *
 * @return Accelerometer Y bias (m/s)
 */
static inline float mavlink_msg_sensor_bias_get_ayBias(const mavlink_message_t* msg)
{
lm's avatar
lm committed
177
	return MAVLINK_MSG_RETURN_float(msg,  4);
James Goppert's avatar
James Goppert committed
178 179 180 181 182 183 184 185 186
}

/**
 * @brief Get field azBias from sensor_bias message
 *
 * @return Accelerometer Z bias (m/s)
 */
static inline float mavlink_msg_sensor_bias_get_azBias(const mavlink_message_t* msg)
{
lm's avatar
lm committed
187
	return MAVLINK_MSG_RETURN_float(msg,  8);
James Goppert's avatar
James Goppert committed
188 189 190 191 192 193 194 195 196
}

/**
 * @brief Get field gxBias from sensor_bias message
 *
 * @return Gyro X bias (rad/s)
 */
static inline float mavlink_msg_sensor_bias_get_gxBias(const mavlink_message_t* msg)
{
lm's avatar
lm committed
197
	return MAVLINK_MSG_RETURN_float(msg,  12);
James Goppert's avatar
James Goppert committed
198 199 200 201 202 203 204 205 206
}

/**
 * @brief Get field gyBias from sensor_bias message
 *
 * @return Gyro Y bias (rad/s)
 */
static inline float mavlink_msg_sensor_bias_get_gyBias(const mavlink_message_t* msg)
{
lm's avatar
lm committed
207
	return MAVLINK_MSG_RETURN_float(msg,  16);
James Goppert's avatar
James Goppert committed
208 209 210 211 212 213 214 215 216
}

/**
 * @brief Get field gzBias from sensor_bias message
 *
 * @return Gyro Z bias (rad/s)
 */
static inline float mavlink_msg_sensor_bias_get_gzBias(const mavlink_message_t* msg)
{
lm's avatar
lm committed
217
	return MAVLINK_MSG_RETURN_float(msg,  20);
James Goppert's avatar
James Goppert committed
218 219
}

James Goppert's avatar
James Goppert committed
220 221 222 223 224 225
/**
 * @brief Decode a sensor_bias message into a struct
 *
 * @param msg The message to decode
 * @param sensor_bias C-struct to decode the message contents into
 */
James Goppert's avatar
James Goppert committed
226 227
static inline void mavlink_msg_sensor_bias_decode(const mavlink_message_t* msg, mavlink_sensor_bias_t* sensor_bias)
{
lm's avatar
lm committed
228 229 230 231 232 233 234 235 236 237
#if MAVLINK_NEED_BYTE_SWAP
	sensor_bias->axBias = mavlink_msg_sensor_bias_get_axBias(msg);
	sensor_bias->ayBias = mavlink_msg_sensor_bias_get_ayBias(msg);
	sensor_bias->azBias = mavlink_msg_sensor_bias_get_azBias(msg);
	sensor_bias->gxBias = mavlink_msg_sensor_bias_get_gxBias(msg);
	sensor_bias->gyBias = mavlink_msg_sensor_bias_get_gyBias(msg);
	sensor_bias->gzBias = mavlink_msg_sensor_bias_get_gzBias(msg);
#else
	memcpy(sensor_bias, MAVLINK_PAYLOAD(msg), 24);
#endif
James Goppert's avatar
James Goppert committed
238
}