mavlink_msg_radio_calibration.h 10.6 KB
Newer Older
James Goppert's avatar
James Goppert committed
1 2
// MESSAGE RADIO_CALIBRATION PACKING

LM's avatar
LM committed
3
#define MAVLINK_MSG_ID_RADIO_CALIBRATION 221
James Goppert's avatar
James Goppert committed
4

lm's avatar
lm committed
5
typedef struct __mavlink_radio_calibration_t
James Goppert's avatar
James Goppert committed
6
{
lm's avatar
lm committed
7 8 9 10 11 12
 uint16_t aileron[3]; ///< Aileron setpoints: left, center, right
 uint16_t elevator[3]; ///< Elevator setpoints: nose down, center, nose up
 uint16_t rudder[3]; ///< Rudder setpoints: nose left, center, nose right
 uint16_t gyro[2]; ///< Tail gyro mode/gain setpoints: heading hold, rate mode
 uint16_t pitch[5]; ///< Pitch curve setpoints (every 25%)
 uint16_t throttle[5]; ///< Throttle curve setpoints (every 25%)
James Goppert's avatar
James Goppert committed
13
} mavlink_radio_calibration_t;
lm's avatar
lm committed
14 15 16 17

#define MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN 42
#define MAVLINK_MSG_ID_221_LEN 42

James Goppert's avatar
James Goppert committed
18 19 20 21 22 23 24
#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_AILERON_LEN 3
#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_ELEVATOR_LEN 3
#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_RUDDER_LEN 3
#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_GYRO_LEN 2
#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_PITCH_LEN 5
#define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_THROTTLE_LEN 5

lm's avatar
lm committed
25 26 27
#define MAVLINK_MESSAGE_INFO_RADIO_CALIBRATION { \
	"RADIO_CALIBRATION", \
	6, \
lm's avatar
lm committed
28 29 30 31 32 33
	{  { "aileron", NULL, MAVLINK_TYPE_UINT16_T, 3, 0, offsetof(mavlink_radio_calibration_t, aileron) }, \
         { "elevator", NULL, MAVLINK_TYPE_UINT16_T, 3, 6, offsetof(mavlink_radio_calibration_t, elevator) }, \
         { "rudder", NULL, MAVLINK_TYPE_UINT16_T, 3, 12, offsetof(mavlink_radio_calibration_t, rudder) }, \
         { "gyro", NULL, MAVLINK_TYPE_UINT16_T, 2, 18, offsetof(mavlink_radio_calibration_t, gyro) }, \
         { "pitch", NULL, MAVLINK_TYPE_UINT16_T, 5, 22, offsetof(mavlink_radio_calibration_t, pitch) }, \
         { "throttle", NULL, MAVLINK_TYPE_UINT16_T, 5, 32, offsetof(mavlink_radio_calibration_t, throttle) }, \
lm's avatar
lm committed
34 35 36 37
         } \
}


James Goppert's avatar
James Goppert committed
38 39 40 41 42 43
/**
 * @brief Pack a radio_calibration 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
 *
LM's avatar
LM committed
44 45 46
 * @param aileron Aileron setpoints: left, center, right
 * @param elevator Elevator setpoints: nose down, center, nose up
 * @param rudder Rudder setpoints: nose left, center, nose right
James Goppert's avatar
James Goppert committed
47 48 49 50 51
 * @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode
 * @param pitch Pitch curve setpoints (every 25%)
 * @param throttle Throttle curve setpoints (every 25%)
 * @return length of the message in bytes (excluding serial stream start sign)
 */
lm's avatar
lm committed
52 53
static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
						       const uint16_t *aileron, const uint16_t *elevator, const uint16_t *rudder, const uint16_t *gyro, const uint16_t *pitch, const uint16_t *throttle)
James Goppert's avatar
James Goppert committed
54
{
LM's avatar
LM committed
55 56 57 58 59 60 61 62 63
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
	char buf[42];

	_mav_put_uint16_t_array(buf, 0, aileron, 3);
	_mav_put_uint16_t_array(buf, 6, elevator, 3);
	_mav_put_uint16_t_array(buf, 12, rudder, 3);
	_mav_put_uint16_t_array(buf, 18, gyro, 2);
	_mav_put_uint16_t_array(buf, 22, pitch, 5);
	_mav_put_uint16_t_array(buf, 32, throttle, 5);
64
        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 42);
LM's avatar
LM committed
65 66
#else
	mavlink_radio_calibration_t packet;
James Goppert's avatar
James Goppert committed
67

68 69 70 71 72 73
	mav_array_memcpy(packet.aileron, aileron, sizeof(uint16_t)*3);
	mav_array_memcpy(packet.elevator, elevator, sizeof(uint16_t)*3);
	mav_array_memcpy(packet.rudder, rudder, sizeof(uint16_t)*3);
	mav_array_memcpy(packet.gyro, gyro, sizeof(uint16_t)*2);
	mav_array_memcpy(packet.pitch, pitch, sizeof(uint16_t)*5);
	mav_array_memcpy(packet.throttle, throttle, sizeof(uint16_t)*5);
74
        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 42);
LM's avatar
LM committed
75
#endif
James Goppert's avatar
James Goppert committed
76

LM's avatar
LM committed
77
	msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION;
lm's avatar
lm committed
78
	return mavlink_finalize_message(msg, system_id, component_id, 42, 71);
James Goppert's avatar
James Goppert committed
79 80 81
}

/**
lm's avatar
lm committed
82
 * @brief Pack a radio_calibration message on a channel
James Goppert's avatar
James Goppert committed
83 84 85 86
 * @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
LM's avatar
LM committed
87 88 89
 * @param aileron Aileron setpoints: left, center, right
 * @param elevator Elevator setpoints: nose down, center, nose up
 * @param rudder Rudder setpoints: nose left, center, nose right
James Goppert's avatar
James Goppert committed
90 91 92 93 94
 * @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode
 * @param pitch Pitch curve setpoints (every 25%)
 * @param throttle Throttle curve setpoints (every 25%)
 * @return length of the message in bytes (excluding serial stream start sign)
 */
lm's avatar
lm committed
95 96 97
static inline uint16_t mavlink_msg_radio_calibration_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
							   mavlink_message_t* msg,
						           const uint16_t *aileron,const uint16_t *elevator,const uint16_t *rudder,const uint16_t *gyro,const uint16_t *pitch,const uint16_t *throttle)
James Goppert's avatar
James Goppert committed
98
{
LM's avatar
LM committed
99 100
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
	char buf[42];
James Goppert's avatar
James Goppert committed
101

LM's avatar
LM committed
102 103 104 105 106 107
	_mav_put_uint16_t_array(buf, 0, aileron, 3);
	_mav_put_uint16_t_array(buf, 6, elevator, 3);
	_mav_put_uint16_t_array(buf, 12, rudder, 3);
	_mav_put_uint16_t_array(buf, 18, gyro, 2);
	_mav_put_uint16_t_array(buf, 22, pitch, 5);
	_mav_put_uint16_t_array(buf, 32, throttle, 5);
108
        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 42);
LM's avatar
LM committed
109 110
#else
	mavlink_radio_calibration_t packet;
James Goppert's avatar
James Goppert committed
111

112 113 114 115 116 117
	mav_array_memcpy(packet.aileron, aileron, sizeof(uint16_t)*3);
	mav_array_memcpy(packet.elevator, elevator, sizeof(uint16_t)*3);
	mav_array_memcpy(packet.rudder, rudder, sizeof(uint16_t)*3);
	mav_array_memcpy(packet.gyro, gyro, sizeof(uint16_t)*2);
	mav_array_memcpy(packet.pitch, pitch, sizeof(uint16_t)*5);
	mav_array_memcpy(packet.throttle, throttle, sizeof(uint16_t)*5);
118
        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 42);
LM's avatar
LM committed
119 120 121
#endif

	msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION;
lm's avatar
lm committed
122
	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 42, 71);
James Goppert's avatar
James Goppert committed
123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141
}

/**
 * @brief Encode a radio_calibration 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 radio_calibration C-struct to read the message contents from
 */
static inline uint16_t mavlink_msg_radio_calibration_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_calibration_t* radio_calibration)
{
	return mavlink_msg_radio_calibration_pack(system_id, component_id, msg, radio_calibration->aileron, radio_calibration->elevator, radio_calibration->rudder, radio_calibration->gyro, radio_calibration->pitch, radio_calibration->throttle);
}

/**
 * @brief Send a radio_calibration message
 * @param chan MAVLink channel to send the message
 *
LM's avatar
LM committed
142 143 144
 * @param aileron Aileron setpoints: left, center, right
 * @param elevator Elevator setpoints: nose down, center, nose up
 * @param rudder Rudder setpoints: nose left, center, nose right
James Goppert's avatar
James Goppert committed
145 146 147 148
 * @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode
 * @param pitch Pitch curve setpoints (every 25%)
 * @param throttle Throttle curve setpoints (every 25%)
 */
lm's avatar
lm committed
149 150 151
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, const uint16_t *aileron, const uint16_t *elevator, const uint16_t *rudder, const uint16_t *gyro, const uint16_t *pitch, const uint16_t *throttle)
lm's avatar
lm committed
152
{
LM's avatar
LM committed
153 154
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
	char buf[42];
LM's avatar
LM committed
155

LM's avatar
LM committed
156 157 158 159 160 161 162 163 164
	_mav_put_uint16_t_array(buf, 0, aileron, 3);
	_mav_put_uint16_t_array(buf, 6, elevator, 3);
	_mav_put_uint16_t_array(buf, 12, rudder, 3);
	_mav_put_uint16_t_array(buf, 18, gyro, 2);
	_mav_put_uint16_t_array(buf, 22, pitch, 5);
	_mav_put_uint16_t_array(buf, 32, throttle, 5);
	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_CALIBRATION, buf, 42, 71);
#else
	mavlink_radio_calibration_t packet;
LM's avatar
LM committed
165

166 167 168 169 170 171
	mav_array_memcpy(packet.aileron, aileron, sizeof(uint16_t)*3);
	mav_array_memcpy(packet.elevator, elevator, sizeof(uint16_t)*3);
	mav_array_memcpy(packet.rudder, rudder, sizeof(uint16_t)*3);
	mav_array_memcpy(packet.gyro, gyro, sizeof(uint16_t)*2);
	mav_array_memcpy(packet.pitch, pitch, sizeof(uint16_t)*5);
	mav_array_memcpy(packet.throttle, throttle, sizeof(uint16_t)*5);
LM's avatar
LM committed
172 173
	_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_CALIBRATION, (const char *)&packet, 42, 71);
#endif
James Goppert's avatar
James Goppert committed
174 175 176
}

#endif
lm's avatar
lm committed
177

James Goppert's avatar
James Goppert committed
178 179
// MESSAGE RADIO_CALIBRATION UNPACKING

lm's avatar
lm committed
180

James Goppert's avatar
James Goppert committed
181 182 183
/**
 * @brief Get field aileron from radio_calibration message
 *
LM's avatar
LM committed
184
 * @return Aileron setpoints: left, center, right
James Goppert's avatar
James Goppert committed
185
 */
lm's avatar
lm committed
186
static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_message_t* msg, uint16_t *aileron)
James Goppert's avatar
James Goppert committed
187
{
LM's avatar
LM committed
188
	return _MAV_RETURN_uint16_t_array(msg, aileron, 3,  0);
James Goppert's avatar
James Goppert committed
189 190 191 192 193
}

/**
 * @brief Get field elevator from radio_calibration message
 *
LM's avatar
LM committed
194
 * @return Elevator setpoints: nose down, center, nose up
James Goppert's avatar
James Goppert committed
195
 */
lm's avatar
lm committed
196
static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_message_t* msg, uint16_t *elevator)
James Goppert's avatar
James Goppert committed
197
{
LM's avatar
LM committed
198
	return _MAV_RETURN_uint16_t_array(msg, elevator, 3,  6);
James Goppert's avatar
James Goppert committed
199 200 201 202 203
}

/**
 * @brief Get field rudder from radio_calibration message
 *
LM's avatar
LM committed
204
 * @return Rudder setpoints: nose left, center, nose right
James Goppert's avatar
James Goppert committed
205
 */
lm's avatar
lm committed
206
static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_message_t* msg, uint16_t *rudder)
James Goppert's avatar
James Goppert committed
207
{
LM's avatar
LM committed
208
	return _MAV_RETURN_uint16_t_array(msg, rudder, 3,  12);
James Goppert's avatar
James Goppert committed
209 210 211 212 213 214 215
}

/**
 * @brief Get field gyro from radio_calibration message
 *
 * @return Tail gyro mode/gain setpoints: heading hold, rate mode
 */
lm's avatar
lm committed
216
static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_message_t* msg, uint16_t *gyro)
James Goppert's avatar
James Goppert committed
217
{
LM's avatar
LM committed
218
	return _MAV_RETURN_uint16_t_array(msg, gyro, 2,  18);
James Goppert's avatar
James Goppert committed
219 220 221 222 223 224 225
}

/**
 * @brief Get field pitch from radio_calibration message
 *
 * @return Pitch curve setpoints (every 25%)
 */
lm's avatar
lm committed
226
static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_message_t* msg, uint16_t *pitch)
James Goppert's avatar
James Goppert committed
227
{
LM's avatar
LM committed
228
	return _MAV_RETURN_uint16_t_array(msg, pitch, 5,  22);
James Goppert's avatar
James Goppert committed
229 230 231 232 233 234 235
}

/**
 * @brief Get field throttle from radio_calibration message
 *
 * @return Throttle curve setpoints (every 25%)
 */
lm's avatar
lm committed
236
static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_message_t* msg, uint16_t *throttle)
James Goppert's avatar
James Goppert committed
237
{
LM's avatar
LM committed
238
	return _MAV_RETURN_uint16_t_array(msg, throttle, 5,  32);
James Goppert's avatar
James Goppert committed
239 240 241 242 243 244 245 246 247 248
}

/**
 * @brief Decode a radio_calibration message into a struct
 *
 * @param msg The message to decode
 * @param radio_calibration C-struct to decode the message contents into
 */
static inline void mavlink_msg_radio_calibration_decode(const mavlink_message_t* msg, mavlink_radio_calibration_t* radio_calibration)
{
lm's avatar
lm committed
249 250 251 252 253 254 255 256
#if MAVLINK_NEED_BYTE_SWAP
	mavlink_msg_radio_calibration_get_aileron(msg, radio_calibration->aileron);
	mavlink_msg_radio_calibration_get_elevator(msg, radio_calibration->elevator);
	mavlink_msg_radio_calibration_get_rudder(msg, radio_calibration->rudder);
	mavlink_msg_radio_calibration_get_gyro(msg, radio_calibration->gyro);
	mavlink_msg_radio_calibration_get_pitch(msg, radio_calibration->pitch);
	mavlink_msg_radio_calibration_get_throttle(msg, radio_calibration->throttle);
#else
LM's avatar
LM committed
257
	memcpy(radio_calibration, _MAV_PAYLOAD(msg), 42);
lm's avatar
lm committed
258
#endif
James Goppert's avatar
James Goppert committed
259
}