mavlink_msg_radio_calibration.h 10.3 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
James Goppert's avatar
James Goppert committed
6 7 8

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

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

lm's avatar
lm committed
43 44 45 46 47 48
	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
49

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

/**
 * @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
59 60 61
 * @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
62 63 64 65 66
 * @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
67
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
68
{
lm's avatar
lm committed
69
	mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0];
James Goppert's avatar
James Goppert committed
70 71
	msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION;

lm's avatar
lm committed
72 73 74 75 76 77
	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
78

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

/**
 * @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
99 100 101
 * @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
102 103 104 105
 * @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
106 107


108
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
lm's avatar
lm committed
109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139
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;
	uint16_t checksum;
	mavlink_radio_calibration_t *p = &payload;

	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%)

	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 );

	crc_init(&checksum);
	checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN);
	checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len );
	hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
	hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte

	mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len);
	mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES);
James Goppert's avatar
James Goppert committed
140 141 142 143 144 145 146 147
}

#endif
// MESSAGE RADIO_CALIBRATION UNPACKING

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

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

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

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

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

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

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

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

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

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

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

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

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