Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.

Dependencies:   Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers mavlink_msg_manual_control.h Source File

mavlink_msg_manual_control.h

00001 // MESSAGE MANUAL_CONTROL PACKING
00002 
00003 #define MAVLINK_MSG_ID_MANUAL_CONTROL 69
00004 
00005 typedef struct __mavlink_manual_control_t 
00006 {
00007     uint8_t target; ///< The system to be controlled
00008     float roll; ///< roll
00009     float pitch; ///< pitch
00010     float yaw; ///< yaw
00011     float thrust; ///< thrust
00012     uint8_t roll_manual; ///< roll control enabled auto:0, manual:1
00013     uint8_t pitch_manual; ///< pitch auto:0, manual:1
00014     uint8_t yaw_manual; ///< yaw auto:0, manual:1
00015     uint8_t thrust_manual; ///< thrust auto:0, manual:1
00016 
00017 } mavlink_manual_control_t;
00018 
00019 
00020 
00021 /**
00022  * @brief Pack a manual_control message
00023  * @param system_id ID of this system
00024  * @param component_id ID of this component (e.g. 200 for IMU)
00025  * @param msg The MAVLink message to compress the data into
00026  *
00027  * @param target The system to be controlled
00028  * @param roll roll
00029  * @param pitch pitch
00030  * @param yaw yaw
00031  * @param thrust thrust
00032  * @param roll_manual roll control enabled auto:0, manual:1
00033  * @param pitch_manual pitch auto:0, manual:1
00034  * @param yaw_manual yaw auto:0, manual:1
00035  * @param thrust_manual thrust auto:0, manual:1
00036  * @return length of the message in bytes (excluding serial stream start sign)
00037  */
00038 static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual)
00039 {
00040     uint16_t i = 0;
00041     msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL;
00042 
00043     i += put_uint8_t_by_index(target, i, msg->payload); // The system to be controlled
00044     i += put_float_by_index(roll, i, msg->payload); // roll
00045     i += put_float_by_index(pitch, i, msg->payload); // pitch
00046     i += put_float_by_index(yaw, i, msg->payload); // yaw
00047     i += put_float_by_index(thrust, i, msg->payload); // thrust
00048     i += put_uint8_t_by_index(roll_manual, i, msg->payload); // roll control enabled auto:0, manual:1
00049     i += put_uint8_t_by_index(pitch_manual, i, msg->payload); // pitch auto:0, manual:1
00050     i += put_uint8_t_by_index(yaw_manual, i, msg->payload); // yaw auto:0, manual:1
00051     i += put_uint8_t_by_index(thrust_manual, i, msg->payload); // thrust auto:0, manual:1
00052 
00053     return mavlink_finalize_message(msg, system_id, component_id, i);
00054 }
00055 
00056 /**
00057  * @brief Pack a manual_control message
00058  * @param system_id ID of this system
00059  * @param component_id ID of this component (e.g. 200 for IMU)
00060  * @param chan The MAVLink channel this message was sent over
00061  * @param msg The MAVLink message to compress the data into
00062  * @param target The system to be controlled
00063  * @param roll roll
00064  * @param pitch pitch
00065  * @param yaw yaw
00066  * @param thrust thrust
00067  * @param roll_manual roll control enabled auto:0, manual:1
00068  * @param pitch_manual pitch auto:0, manual:1
00069  * @param yaw_manual yaw auto:0, manual:1
00070  * @param thrust_manual thrust auto:0, manual:1
00071  * @return length of the message in bytes (excluding serial stream start sign)
00072  */
00073 static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual)
00074 {
00075     uint16_t i = 0;
00076     msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL;
00077 
00078     i += put_uint8_t_by_index(target, i, msg->payload); // The system to be controlled
00079     i += put_float_by_index(roll, i, msg->payload); // roll
00080     i += put_float_by_index(pitch, i, msg->payload); // pitch
00081     i += put_float_by_index(yaw, i, msg->payload); // yaw
00082     i += put_float_by_index(thrust, i, msg->payload); // thrust
00083     i += put_uint8_t_by_index(roll_manual, i, msg->payload); // roll control enabled auto:0, manual:1
00084     i += put_uint8_t_by_index(pitch_manual, i, msg->payload); // pitch auto:0, manual:1
00085     i += put_uint8_t_by_index(yaw_manual, i, msg->payload); // yaw auto:0, manual:1
00086     i += put_uint8_t_by_index(thrust_manual, i, msg->payload); // thrust auto:0, manual:1
00087 
00088     return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
00089 }
00090 
00091 /**
00092  * @brief Encode a manual_control struct into a message
00093  *
00094  * @param system_id ID of this system
00095  * @param component_id ID of this component (e.g. 200 for IMU)
00096  * @param msg The MAVLink message to compress the data into
00097  * @param manual_control C-struct to read the message contents from
00098  */
00099 static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control)
00100 {
00101     return mavlink_msg_manual_control_pack(system_id, component_id, msg, manual_control->target, manual_control->roll, manual_control->pitch, manual_control->yaw, manual_control->thrust, manual_control->roll_manual, manual_control->pitch_manual, manual_control->yaw_manual, manual_control->thrust_manual);
00102 }
00103 
00104 /**
00105  * @brief Send a manual_control message
00106  * @param chan MAVLink channel to send the message
00107  *
00108  * @param target The system to be controlled
00109  * @param roll roll
00110  * @param pitch pitch
00111  * @param yaw yaw
00112  * @param thrust thrust
00113  * @param roll_manual roll control enabled auto:0, manual:1
00114  * @param pitch_manual pitch auto:0, manual:1
00115  * @param yaw_manual yaw auto:0, manual:1
00116  * @param thrust_manual thrust auto:0, manual:1
00117  */
00118 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00119 
00120 static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual)
00121 {
00122     mavlink_message_t msg;
00123     mavlink_msg_manual_control_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual);
00124     mavlink_send_uart(chan, &msg);
00125 }
00126 
00127 #endif
00128 // MESSAGE MANUAL_CONTROL UNPACKING
00129 
00130 /**
00131  * @brief Get field target from manual_control message
00132  *
00133  * @return The system to be controlled
00134  */
00135 static inline uint8_t mavlink_msg_manual_control_get_target(const mavlink_message_t* msg)
00136 {
00137     return (uint8_t)(msg->payload)[0];
00138 }
00139 
00140 /**
00141  * @brief Get field roll from manual_control message
00142  *
00143  * @return roll
00144  */
00145 static inline float mavlink_msg_manual_control_get_roll(const mavlink_message_t* msg)
00146 {
00147     generic_32bit r;
00148     r.b[3] = (msg->payload+sizeof(uint8_t))[0];
00149     r.b[2] = (msg->payload+sizeof(uint8_t))[1];
00150     r.b[1] = (msg->payload+sizeof(uint8_t))[2];
00151     r.b[0] = (msg->payload+sizeof(uint8_t))[3];
00152     return (float)r.f;
00153 }
00154 
00155 /**
00156  * @brief Get field pitch from manual_control message
00157  *
00158  * @return pitch
00159  */
00160 static inline float mavlink_msg_manual_control_get_pitch(const mavlink_message_t* msg)
00161 {
00162     generic_32bit r;
00163     r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0];
00164     r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1];
00165     r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2];
00166     r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3];
00167     return (float)r.f;
00168 }
00169 
00170 /**
00171  * @brief Get field yaw from manual_control message
00172  *
00173  * @return yaw
00174  */
00175 static inline float mavlink_msg_manual_control_get_yaw(const mavlink_message_t* msg)
00176 {
00177     generic_32bit r;
00178     r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0];
00179     r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1];
00180     r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2];
00181     r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3];
00182     return (float)r.f;
00183 }
00184 
00185 /**
00186  * @brief Get field thrust from manual_control message
00187  *
00188  * @return thrust
00189  */
00190 static inline float mavlink_msg_manual_control_get_thrust(const mavlink_message_t* msg)
00191 {
00192     generic_32bit r;
00193     r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0];
00194     r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1];
00195     r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2];
00196     r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3];
00197     return (float)r.f;
00198 }
00199 
00200 /**
00201  * @brief Get field roll_manual from manual_control message
00202  *
00203  * @return roll control enabled auto:0, manual:1
00204  */
00205 static inline uint8_t mavlink_msg_manual_control_get_roll_manual(const mavlink_message_t* msg)
00206 {
00207     return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
00208 }
00209 
00210 /**
00211  * @brief Get field pitch_manual from manual_control message
00212  *
00213  * @return pitch auto:0, manual:1
00214  */
00215 static inline uint8_t mavlink_msg_manual_control_get_pitch_manual(const mavlink_message_t* msg)
00216 {
00217     return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0];
00218 }
00219 
00220 /**
00221  * @brief Get field yaw_manual from manual_control message
00222  *
00223  * @return yaw auto:0, manual:1
00224  */
00225 static inline uint8_t mavlink_msg_manual_control_get_yaw_manual(const mavlink_message_t* msg)
00226 {
00227     return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t))[0];
00228 }
00229 
00230 /**
00231  * @brief Get field thrust_manual from manual_control message
00232  *
00233  * @return thrust auto:0, manual:1
00234  */
00235 static inline uint8_t mavlink_msg_manual_control_get_thrust_manual(const mavlink_message_t* msg)
00236 {
00237     return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0];
00238 }
00239 
00240 /**
00241  * @brief Decode a manual_control message into a struct
00242  *
00243  * @param msg The message to decode
00244  * @param manual_control C-struct to decode the message contents into
00245  */
00246 static inline void mavlink_msg_manual_control_decode(const mavlink_message_t* msg, mavlink_manual_control_t* manual_control)
00247 {
00248     manual_control->target = mavlink_msg_manual_control_get_target(msg);
00249     manual_control->roll = mavlink_msg_manual_control_get_roll(msg);
00250     manual_control->pitch = mavlink_msg_manual_control_get_pitch(msg);
00251     manual_control->yaw = mavlink_msg_manual_control_get_yaw(msg);
00252     manual_control->thrust = mavlink_msg_manual_control_get_thrust(msg);
00253     manual_control->roll_manual = mavlink_msg_manual_control_get_roll_manual(msg);
00254     manual_control->pitch_manual = mavlink_msg_manual_control_get_pitch_manual(msg);
00255     manual_control->yaw_manual = mavlink_msg_manual_control_get_yaw_manual(msg);
00256     manual_control->thrust_manual = mavlink_msg_manual_control_get_thrust_manual(msg);
00257 }