Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
mavlink_msg_radio_calibration.h
00001 // MESSAGE RADIO_CALIBRATION PACKING 00002 00003 #define MAVLINK_MSG_ID_RADIO_CALIBRATION 221 00004 00005 typedef struct __mavlink_radio_calibration_t 00006 { 00007 uint16_t aileron[3]; ///< Aileron setpoints: left, center, right 00008 uint16_t elevator[3]; ///< Elevator setpoints: nose down, center, nose up 00009 uint16_t rudder[3]; ///< Rudder setpoints: nose left, center, nose right 00010 uint16_t gyro[2]; ///< Tail gyro mode/gain setpoints: heading hold, rate mode 00011 uint16_t pitch[5]; ///< Pitch curve setpoints (every 25%) 00012 uint16_t throttle[5]; ///< Throttle curve setpoints (every 25%) 00013 00014 } mavlink_radio_calibration_t; 00015 00016 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_AILERON_LEN 3 00017 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_ELEVATOR_LEN 3 00018 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_RUDDER_LEN 3 00019 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_GYRO_LEN 2 00020 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_PITCH_LEN 5 00021 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_THROTTLE_LEN 5 00022 00023 00024 /** 00025 * @brief Pack a radio_calibration message 00026 * @param system_id ID of this system 00027 * @param component_id ID of this component (e.g. 200 for IMU) 00028 * @param msg The MAVLink message to compress the data into 00029 * 00030 * @param aileron Aileron setpoints: left, center, right 00031 * @param elevator Elevator setpoints: nose down, center, nose up 00032 * @param rudder Rudder setpoints: nose left, center, nose right 00033 * @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode 00034 * @param pitch Pitch curve setpoints (every 25%) 00035 * @param throttle Throttle curve setpoints (every 25%) 00036 * @return length of the message in bytes (excluding serial stream start sign) 00037 */ 00038 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) 00039 { 00040 uint16_t i = 0; 00041 msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; 00042 00043 i += put_array_by_index((const int8_t*)aileron, sizeof(uint16_t)*3, i, msg->payload); // Aileron setpoints: left, center, right 00044 i += put_array_by_index((const int8_t*)elevator, sizeof(uint16_t)*3, i, msg->payload); // Elevator setpoints: nose down, center, nose up 00045 i += put_array_by_index((const int8_t*)rudder, sizeof(uint16_t)*3, i, msg->payload); // Rudder setpoints: nose left, center, nose right 00046 i += put_array_by_index((const int8_t*)gyro, sizeof(uint16_t)*2, i, msg->payload); // Tail gyro mode/gain setpoints: heading hold, rate mode 00047 i += put_array_by_index((const int8_t*)pitch, sizeof(uint16_t)*5, i, msg->payload); // Pitch curve setpoints (every 25%) 00048 i += put_array_by_index((const int8_t*)throttle, sizeof(uint16_t)*5, i, msg->payload); // Throttle curve setpoints (every 25%) 00049 00050 return mavlink_finalize_message(msg, system_id, component_id, i); 00051 } 00052 00053 /** 00054 * @brief Pack a radio_calibration message 00055 * @param system_id ID of this system 00056 * @param component_id ID of this component (e.g. 200 for IMU) 00057 * @param chan The MAVLink channel this message was sent over 00058 * @param msg The MAVLink message to compress the data into 00059 * @param aileron Aileron setpoints: left, center, right 00060 * @param elevator Elevator setpoints: nose down, center, nose up 00061 * @param rudder Rudder setpoints: nose left, center, nose right 00062 * @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode 00063 * @param pitch Pitch curve setpoints (every 25%) 00064 * @param throttle Throttle curve setpoints (every 25%) 00065 * @return length of the message in bytes (excluding serial stream start sign) 00066 */ 00067 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) 00068 { 00069 uint16_t i = 0; 00070 msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; 00071 00072 i += put_array_by_index((const int8_t*)aileron, sizeof(uint16_t)*3, i, msg->payload); // Aileron setpoints: left, center, right 00073 i += put_array_by_index((const int8_t*)elevator, sizeof(uint16_t)*3, i, msg->payload); // Elevator setpoints: nose down, center, nose up 00074 i += put_array_by_index((const int8_t*)rudder, sizeof(uint16_t)*3, i, msg->payload); // Rudder setpoints: nose left, center, nose right 00075 i += put_array_by_index((const int8_t*)gyro, sizeof(uint16_t)*2, i, msg->payload); // Tail gyro mode/gain setpoints: heading hold, rate mode 00076 i += put_array_by_index((const int8_t*)pitch, sizeof(uint16_t)*5, i, msg->payload); // Pitch curve setpoints (every 25%) 00077 i += put_array_by_index((const int8_t*)throttle, sizeof(uint16_t)*5, i, msg->payload); // Throttle curve setpoints (every 25%) 00078 00079 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); 00080 } 00081 00082 /** 00083 * @brief Encode a radio_calibration struct into a message 00084 * 00085 * @param system_id ID of this system 00086 * @param component_id ID of this component (e.g. 200 for IMU) 00087 * @param msg The MAVLink message to compress the data into 00088 * @param radio_calibration C-struct to read the message contents from 00089 */ 00090 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) 00091 { 00092 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); 00093 } 00094 00095 /** 00096 * @brief Send a radio_calibration message 00097 * @param chan MAVLink channel to send the message 00098 * 00099 * @param aileron Aileron setpoints: left, center, right 00100 * @param elevator Elevator setpoints: nose down, center, nose up 00101 * @param rudder Rudder setpoints: nose left, center, nose right 00102 * @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode 00103 * @param pitch Pitch curve setpoints (every 25%) 00104 * @param throttle Throttle curve setpoints (every 25%) 00105 */ 00106 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 00107 00108 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) 00109 { 00110 mavlink_message_t msg; 00111 mavlink_msg_radio_calibration_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, aileron, elevator, rudder, gyro, pitch, throttle); 00112 mavlink_send_uart(chan, &msg); 00113 } 00114 00115 #endif 00116 // MESSAGE RADIO_CALIBRATION UNPACKING 00117 00118 /** 00119 * @brief Get field aileron from radio_calibration message 00120 * 00121 * @return Aileron setpoints: left, center, right 00122 */ 00123 static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_message_t* msg, uint16_t* r_data) 00124 { 00125 00126 memcpy(r_data, msg->payload, sizeof(uint16_t)*3); 00127 return sizeof(uint16_t)*3; 00128 } 00129 00130 /** 00131 * @brief Get field elevator from radio_calibration message 00132 * 00133 * @return Elevator setpoints: nose down, center, nose up 00134 */ 00135 static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_message_t* msg, uint16_t* r_data) 00136 { 00137 00138 memcpy(r_data, msg->payload+sizeof(uint16_t)*3, sizeof(uint16_t)*3); 00139 return sizeof(uint16_t)*3; 00140 } 00141 00142 /** 00143 * @brief Get field rudder from radio_calibration message 00144 * 00145 * @return Rudder setpoints: nose left, center, nose right 00146 */ 00147 static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_message_t* msg, uint16_t* r_data) 00148 { 00149 00150 memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3, sizeof(uint16_t)*3); 00151 return sizeof(uint16_t)*3; 00152 } 00153 00154 /** 00155 * @brief Get field gyro from radio_calibration message 00156 * 00157 * @return Tail gyro mode/gain setpoints: heading hold, rate mode 00158 */ 00159 static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_message_t* msg, uint16_t* r_data) 00160 { 00161 00162 memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3, sizeof(uint16_t)*2); 00163 return sizeof(uint16_t)*2; 00164 } 00165 00166 /** 00167 * @brief Get field pitch from radio_calibration message 00168 * 00169 * @return Pitch curve setpoints (every 25%) 00170 */ 00171 static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_message_t* msg, uint16_t* r_data) 00172 { 00173 00174 memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*2, sizeof(uint16_t)*5); 00175 return sizeof(uint16_t)*5; 00176 } 00177 00178 /** 00179 * @brief Get field throttle from radio_calibration message 00180 * 00181 * @return Throttle curve setpoints (every 25%) 00182 */ 00183 static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_message_t* msg, uint16_t* r_data) 00184 { 00185 00186 memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*2+sizeof(uint16_t)*5, sizeof(uint16_t)*5); 00187 return sizeof(uint16_t)*5; 00188 } 00189 00190 /** 00191 * @brief Decode a radio_calibration message into a struct 00192 * 00193 * @param msg The message to decode 00194 * @param radio_calibration C-struct to decode the message contents into 00195 */ 00196 static inline void mavlink_msg_radio_calibration_decode(const mavlink_message_t* msg, mavlink_radio_calibration_t* radio_calibration) 00197 { 00198 mavlink_msg_radio_calibration_get_aileron(msg, radio_calibration->aileron); 00199 mavlink_msg_radio_calibration_get_elevator(msg, radio_calibration->elevator); 00200 mavlink_msg_radio_calibration_get_rudder(msg, radio_calibration->rudder); 00201 mavlink_msg_radio_calibration_get_gyro(msg, radio_calibration->gyro); 00202 mavlink_msg_radio_calibration_get_pitch(msg, radio_calibration->pitch); 00203 mavlink_msg_radio_calibration_get_throttle(msg, radio_calibration->throttle); 00204 }
Generated on Tue Jul 12 2022 14:09:26 by 1.7.2