mavlink_msg_radio_calibration.h 10.5 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
lm's avatar
lm committed
4 5
#define MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN 42
#define MAVLINK_MSG_221_LEN 42
lm's avatar
lm committed
6 7
#define MAVLINK_MSG_ID_RADIO_CALIBRATION_KEY 0x49
#define MAVLINK_MSG_221_KEY 0x49
James Goppert's avatar
James Goppert committed
8 9 10

typedef struct __mavlink_radio_calibration_t 
{
lm's avatar
lm committed
11 12 13 14 15 16
	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
17 18 19 20 21 22 23 24 25 26 27 28 29 30 31

} mavlink_radio_calibration_t;
#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

/**
 * @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
32 33 34
 * @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
35 36 37 38 39
 * @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
40
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
41
{
lm's avatar
lm committed
42
	mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0];
James Goppert's avatar
James Goppert committed
43 44
	msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION;

lm's avatar
lm committed
45 46 47 48 49 50
	memcpy(p->aileron, aileron, sizeof(p->aileron));	// uint16_t[3]:Aileron setpoints: left, center, right
	memcpy(p->elevator, elevator, sizeof(p->elevator));	// uint16_t[3]:Elevator setpoints: nose down, center, nose up
	memcpy(p->rudder, rudder, sizeof(p->rudder));	// uint16_t[3]:Rudder setpoints: nose left, center, nose right
	memcpy(p->gyro, gyro, sizeof(p->gyro));	// uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode
	memcpy(p->pitch, pitch, sizeof(p->pitch));	// uint16_t[5]:Pitch curve setpoints (every 25%)
	memcpy(p->throttle, throttle, sizeof(p->throttle));	// uint16_t[5]:Throttle curve setpoints (every 25%)
James Goppert's avatar
James Goppert committed
51

lm's avatar
lm committed
52
	return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN);
James Goppert's avatar
James Goppert committed
53 54 55 56 57 58 59 60
}

/**
 * @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 chan The MAVLink channel this message was sent over
 * @param msg The MAVLink message to compress the data into
LM's avatar
LM committed
61 62 63
 * @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
64 65 66 67 68
 * @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
69
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
70
{
lm's avatar
lm committed
71
	mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0];
James Goppert's avatar
James Goppert committed
72 73
	msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION;

lm's avatar
lm committed
74 75 76 77 78 79
	memcpy(p->aileron, aileron, sizeof(p->aileron));	// uint16_t[3]:Aileron setpoints: left, center, right
	memcpy(p->elevator, elevator, sizeof(p->elevator));	// uint16_t[3]:Elevator setpoints: nose down, center, nose up
	memcpy(p->rudder, rudder, sizeof(p->rudder));	// uint16_t[3]:Rudder setpoints: nose left, center, nose right
	memcpy(p->gyro, gyro, sizeof(p->gyro));	// uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode
	memcpy(p->pitch, pitch, sizeof(p->pitch));	// uint16_t[5]:Pitch curve setpoints (every 25%)
	memcpy(p->throttle, throttle, sizeof(p->throttle));	// uint16_t[5]:Throttle curve setpoints (every 25%)
James Goppert's avatar
James Goppert committed
80

lm's avatar
lm committed
81
	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN);
James Goppert's avatar
James Goppert committed
82 83 84 85 86 87 88 89 90 91 92 93 94 95 96
}

/**
 * @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);
}

lm's avatar
lm committed
97 98

#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
James Goppert's avatar
James Goppert committed
99 100 101 102
/**
 * @brief Send a radio_calibration message
 * @param chan MAVLink channel to send the message
 *
LM's avatar
LM committed
103 104 105
 * @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
106 107 108 109
 * @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
110 111 112 113 114
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)
{
	mavlink_header_t hdr;
	mavlink_radio_calibration_t payload;

lm's avatar
lm committed
115 116 117 118 119 120 121
	MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN )
	memcpy(payload.aileron, aileron, sizeof(payload.aileron));	// uint16_t[3]:Aileron setpoints: left, center, right
	memcpy(payload.elevator, elevator, sizeof(payload.elevator));	// uint16_t[3]:Elevator setpoints: nose down, center, nose up
	memcpy(payload.rudder, rudder, sizeof(payload.rudder));	// uint16_t[3]:Rudder setpoints: nose left, center, nose right
	memcpy(payload.gyro, gyro, sizeof(payload.gyro));	// uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode
	memcpy(payload.pitch, pitch, sizeof(payload.pitch));	// uint16_t[5]:Pitch curve setpoints (every 25%)
	memcpy(payload.throttle, throttle, sizeof(payload.throttle));	// uint16_t[5]:Throttle curve setpoints (every 25%)
lm's avatar
lm committed
122 123 124 125 126 127 128 129 130

	hdr.STX = MAVLINK_STX;
	hdr.len = MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN;
	hdr.msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION;
	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 );
131
	mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) );
lm's avatar
lm committed
132

lm's avatar
lm committed
133 134 135 136 137 138
	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 );
	crc_accumulate( 0x49, &hdr.ck); /// include key in X25 checksum
	mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES);
	MAVLINK_BUFFER_CHECK_END
James Goppert's avatar
James Goppert committed
139 140 141 142 143 144 145 146
}

#endif
// MESSAGE RADIO_CALIBRATION UNPACKING

/**
 * @brief Get field aileron from radio_calibration message
 *
LM's avatar
LM committed
147
 * @return Aileron setpoints: left, center, right
James Goppert's avatar
James Goppert committed
148
 */
lm's avatar
lm committed
149
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
150
{
lm's avatar
lm committed
151
	mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0];
James Goppert's avatar
James Goppert committed
152

lm's avatar
lm committed
153 154
	memcpy(aileron, p->aileron, sizeof(p->aileron));
	return sizeof(p->aileron);
James Goppert's avatar
James Goppert committed
155 156 157 158 159
}

/**
 * @brief Get field elevator from radio_calibration message
 *
LM's avatar
LM committed
160
 * @return Elevator setpoints: nose down, center, nose up
James Goppert's avatar
James Goppert committed
161
 */
lm's avatar
lm committed
162
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
163
{
lm's avatar
lm committed
164
	mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0];
James Goppert's avatar
James Goppert committed
165

lm's avatar
lm committed
166 167
	memcpy(elevator, p->elevator, sizeof(p->elevator));
	return sizeof(p->elevator);
James Goppert's avatar
James Goppert committed
168 169 170 171 172
}

/**
 * @brief Get field rudder from radio_calibration message
 *
LM's avatar
LM committed
173
 * @return Rudder setpoints: nose left, center, nose right
James Goppert's avatar
James Goppert committed
174
 */
lm's avatar
lm committed
175
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
176
{
lm's avatar
lm committed
177
	mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0];
James Goppert's avatar
James Goppert committed
178

lm's avatar
lm committed
179 180
	memcpy(rudder, p->rudder, sizeof(p->rudder));
	return sizeof(p->rudder);
James Goppert's avatar
James Goppert committed
181 182 183 184 185 186 187
}

/**
 * @brief Get field gyro from radio_calibration message
 *
 * @return Tail gyro mode/gain setpoints: heading hold, rate mode
 */
lm's avatar
lm committed
188
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
189
{
lm's avatar
lm committed
190
	mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0];
James Goppert's avatar
James Goppert committed
191

lm's avatar
lm committed
192 193
	memcpy(gyro, p->gyro, sizeof(p->gyro));
	return sizeof(p->gyro);
James Goppert's avatar
James Goppert committed
194 195 196 197 198 199 200
}

/**
 * @brief Get field pitch from radio_calibration message
 *
 * @return Pitch curve setpoints (every 25%)
 */
lm's avatar
lm committed
201
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
202
{
lm's avatar
lm committed
203
	mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0];
James Goppert's avatar
James Goppert committed
204

lm's avatar
lm committed
205 206
	memcpy(pitch, p->pitch, sizeof(p->pitch));
	return sizeof(p->pitch);
James Goppert's avatar
James Goppert committed
207 208 209 210 211 212 213
}

/**
 * @brief Get field throttle from radio_calibration message
 *
 * @return Throttle curve setpoints (every 25%)
 */
lm's avatar
lm committed
214
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
215
{
lm's avatar
lm committed
216
	mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0];
James Goppert's avatar
James Goppert committed
217

lm's avatar
lm committed
218 219
	memcpy(throttle, p->throttle, sizeof(p->throttle));
	return sizeof(p->throttle);
James Goppert's avatar
James Goppert committed
220 221 222 223 224 225 226 227 228 229
}

/**
 * @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
230
	memcpy( radio_calibration, msg->payload, sizeof(mavlink_radio_calibration_t));
James Goppert's avatar
James Goppert committed
231
}