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_set_roll_pitch_yaw_speed.h Source File

mavlink_msg_set_roll_pitch_yaw_speed.h

00001 // MESSAGE SET_ROLL_PITCH_YAW_SPEED PACKING
00002 
00003 #define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED 56
00004 
00005 typedef struct __mavlink_set_roll_pitch_yaw_speed_t 
00006 {
00007     uint8_t target_system; ///< System ID
00008     uint8_t target_component; ///< Component ID
00009     float roll_speed; ///< Desired roll angular speed in rad/s
00010     float pitch_speed; ///< Desired pitch angular speed in rad/s
00011     float yaw_speed; ///< Desired yaw angular speed in rad/s
00012 
00013 } mavlink_set_roll_pitch_yaw_speed_t;
00014 
00015 
00016 
00017 /**
00018  * @brief Pack a set_roll_pitch_yaw_speed message
00019  * @param system_id ID of this system
00020  * @param component_id ID of this component (e.g. 200 for IMU)
00021  * @param msg The MAVLink message to compress the data into
00022  *
00023  * @param target_system System ID
00024  * @param target_component Component ID
00025  * @param roll_speed Desired roll angular speed in rad/s
00026  * @param pitch_speed Desired pitch angular speed in rad/s
00027  * @param yaw_speed Desired yaw angular speed in rad/s
00028  * @return length of the message in bytes (excluding serial stream start sign)
00029  */
00030 static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed)
00031 {
00032     uint16_t i = 0;
00033     msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED;
00034 
00035     i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
00036     i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
00037     i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s
00038     i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s
00039     i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s
00040 
00041     return mavlink_finalize_message(msg, system_id, component_id, i);
00042 }
00043 
00044 /**
00045  * @brief Pack a set_roll_pitch_yaw_speed message
00046  * @param system_id ID of this system
00047  * @param component_id ID of this component (e.g. 200 for IMU)
00048  * @param chan The MAVLink channel this message was sent over
00049  * @param msg The MAVLink message to compress the data into
00050  * @param target_system System ID
00051  * @param target_component Component ID
00052  * @param roll_speed Desired roll angular speed in rad/s
00053  * @param pitch_speed Desired pitch angular speed in rad/s
00054  * @param yaw_speed Desired yaw angular speed in rad/s
00055  * @return length of the message in bytes (excluding serial stream start sign)
00056  */
00057 static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed)
00058 {
00059     uint16_t i = 0;
00060     msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED;
00061 
00062     i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
00063     i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
00064     i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s
00065     i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s
00066     i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s
00067 
00068     return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
00069 }
00070 
00071 /**
00072  * @brief Encode a set_roll_pitch_yaw_speed struct into a message
00073  *
00074  * @param system_id ID of this system
00075  * @param component_id ID of this component (e.g. 200 for IMU)
00076  * @param msg The MAVLink message to compress the data into
00077  * @param set_roll_pitch_yaw_speed C-struct to read the message contents from
00078  */
00079 static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed)
00080 {
00081     return mavlink_msg_set_roll_pitch_yaw_speed_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed->target_system, set_roll_pitch_yaw_speed->target_component, set_roll_pitch_yaw_speed->roll_speed, set_roll_pitch_yaw_speed->pitch_speed, set_roll_pitch_yaw_speed->yaw_speed);
00082 }
00083 
00084 /**
00085  * @brief Send a set_roll_pitch_yaw_speed message
00086  * @param chan MAVLink channel to send the message
00087  *
00088  * @param target_system System ID
00089  * @param target_component Component ID
00090  * @param roll_speed Desired roll angular speed in rad/s
00091  * @param pitch_speed Desired pitch angular speed in rad/s
00092  * @param yaw_speed Desired yaw angular speed in rad/s
00093  */
00094 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00095 
00096 static inline void mavlink_msg_set_roll_pitch_yaw_speed_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed)
00097 {
00098     mavlink_message_t msg;
00099     mavlink_msg_set_roll_pitch_yaw_speed_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll_speed, pitch_speed, yaw_speed);
00100     mavlink_send_uart(chan, &msg);
00101 }
00102 
00103 #endif
00104 // MESSAGE SET_ROLL_PITCH_YAW_SPEED UNPACKING
00105 
00106 /**
00107  * @brief Get field target_system from set_roll_pitch_yaw_speed message
00108  *
00109  * @return System ID
00110  */
00111 static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_system(const mavlink_message_t* msg)
00112 {
00113     return (uint8_t)(msg->payload)[0];
00114 }
00115 
00116 /**
00117  * @brief Get field target_component from set_roll_pitch_yaw_speed message
00118  *
00119  * @return Component ID
00120  */
00121 static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_component(const mavlink_message_t* msg)
00122 {
00123     return (uint8_t)(msg->payload+sizeof(uint8_t))[0];
00124 }
00125 
00126 /**
00127  * @brief Get field roll_speed from set_roll_pitch_yaw_speed message
00128  *
00129  * @return Desired roll angular speed in rad/s
00130  */
00131 static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_roll_speed(const mavlink_message_t* msg)
00132 {
00133     generic_32bit r;
00134     r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0];
00135     r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1];
00136     r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2];
00137     r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3];
00138     return (float)r.f;
00139 }
00140 
00141 /**
00142  * @brief Get field pitch_speed from set_roll_pitch_yaw_speed message
00143  *
00144  * @return Desired pitch angular speed in rad/s
00145  */
00146 static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_pitch_speed(const mavlink_message_t* msg)
00147 {
00148     generic_32bit r;
00149     r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0];
00150     r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1];
00151     r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2];
00152     r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3];
00153     return (float)r.f;
00154 }
00155 
00156 /**
00157  * @brief Get field yaw_speed from set_roll_pitch_yaw_speed message
00158  *
00159  * @return Desired yaw angular speed in rad/s
00160  */
00161 static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_yaw_speed(const mavlink_message_t* msg)
00162 {
00163     generic_32bit r;
00164     r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0];
00165     r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1];
00166     r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2];
00167     r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3];
00168     return (float)r.f;
00169 }
00170 
00171 /**
00172  * @brief Decode a set_roll_pitch_yaw_speed message into a struct
00173  *
00174  * @param msg The message to decode
00175  * @param set_roll_pitch_yaw_speed C-struct to decode the message contents into
00176  */
00177 static inline void mavlink_msg_set_roll_pitch_yaw_speed_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed)
00178 {
00179     set_roll_pitch_yaw_speed->target_system = mavlink_msg_set_roll_pitch_yaw_speed_get_target_system(msg);
00180     set_roll_pitch_yaw_speed->target_component = mavlink_msg_set_roll_pitch_yaw_speed_get_target_component(msg);
00181     set_roll_pitch_yaw_speed->roll_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_roll_speed(msg);
00182     set_roll_pitch_yaw_speed->pitch_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_pitch_speed(msg);
00183     set_roll_pitch_yaw_speed->yaw_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_yaw_speed(msg);
00184 }