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

mavlink_msg_hil_controls.h

00001 // MESSAGE HIL_CONTROLS PACKING
00002 
00003 #define MAVLINK_MSG_ID_HIL_CONTROLS 68
00004 
00005 typedef struct __mavlink_hil_controls_t 
00006 {
00007     uint64_t time_us; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
00008     float roll_ailerons; ///< Control output -3 .. 1
00009     float pitch_elevator; ///< Control output -1 .. 1
00010     float yaw_rudder; ///< Control output -1 .. 1
00011     float throttle; ///< Throttle 0 .. 1
00012     uint8_t mode; ///< System mode (MAV_MODE)
00013     uint8_t nav_mode; ///< Navigation mode (MAV_NAV_MODE)
00014 
00015 } mavlink_hil_controls_t;
00016 
00017 
00018 
00019 /**
00020  * @brief Pack a hil_controls message
00021  * @param system_id ID of this system
00022  * @param component_id ID of this component (e.g. 200 for IMU)
00023  * @param msg The MAVLink message to compress the data into
00024  *
00025  * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot)
00026  * @param roll_ailerons Control output -3 .. 1
00027  * @param pitch_elevator Control output -1 .. 1
00028  * @param yaw_rudder Control output -1 .. 1
00029  * @param throttle Throttle 0 .. 1
00030  * @param mode System mode (MAV_MODE)
00031  * @param nav_mode Navigation mode (MAV_NAV_MODE)
00032  * @return length of the message in bytes (excluding serial stream start sign)
00033  */
00034 static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, uint8_t mode, uint8_t nav_mode)
00035 {
00036     uint16_t i = 0;
00037     msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS;
00038 
00039     i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot)
00040     i += put_float_by_index(roll_ailerons, i, msg->payload); // Control output -3 .. 1
00041     i += put_float_by_index(pitch_elevator, i, msg->payload); // Control output -1 .. 1
00042     i += put_float_by_index(yaw_rudder, i, msg->payload); // Control output -1 .. 1
00043     i += put_float_by_index(throttle, i, msg->payload); // Throttle 0 .. 1
00044     i += put_uint8_t_by_index(mode, i, msg->payload); // System mode (MAV_MODE)
00045     i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode (MAV_NAV_MODE)
00046 
00047     return mavlink_finalize_message(msg, system_id, component_id, i);
00048 }
00049 
00050 /**
00051  * @brief Pack a hil_controls message
00052  * @param system_id ID of this system
00053  * @param component_id ID of this component (e.g. 200 for IMU)
00054  * @param chan The MAVLink channel this message was sent over
00055  * @param msg The MAVLink message to compress the data into
00056  * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot)
00057  * @param roll_ailerons Control output -3 .. 1
00058  * @param pitch_elevator Control output -1 .. 1
00059  * @param yaw_rudder Control output -1 .. 1
00060  * @param throttle Throttle 0 .. 1
00061  * @param mode System mode (MAV_MODE)
00062  * @param nav_mode Navigation mode (MAV_NAV_MODE)
00063  * @return length of the message in bytes (excluding serial stream start sign)
00064  */
00065 static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, uint8_t mode, uint8_t nav_mode)
00066 {
00067     uint16_t i = 0;
00068     msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS;
00069 
00070     i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot)
00071     i += put_float_by_index(roll_ailerons, i, msg->payload); // Control output -3 .. 1
00072     i += put_float_by_index(pitch_elevator, i, msg->payload); // Control output -1 .. 1
00073     i += put_float_by_index(yaw_rudder, i, msg->payload); // Control output -1 .. 1
00074     i += put_float_by_index(throttle, i, msg->payload); // Throttle 0 .. 1
00075     i += put_uint8_t_by_index(mode, i, msg->payload); // System mode (MAV_MODE)
00076     i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode (MAV_NAV_MODE)
00077 
00078     return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
00079 }
00080 
00081 /**
00082  * @brief Encode a hil_controls struct into a message
00083  *
00084  * @param system_id ID of this system
00085  * @param component_id ID of this component (e.g. 200 for IMU)
00086  * @param msg The MAVLink message to compress the data into
00087  * @param hil_controls C-struct to read the message contents from
00088  */
00089 static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_controls_t* hil_controls)
00090 {
00091     return mavlink_msg_hil_controls_pack(system_id, component_id, msg, hil_controls->time_us, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->mode, hil_controls->nav_mode);
00092 }
00093 
00094 /**
00095  * @brief Send a hil_controls message
00096  * @param chan MAVLink channel to send the message
00097  *
00098  * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot)
00099  * @param roll_ailerons Control output -3 .. 1
00100  * @param pitch_elevator Control output -1 .. 1
00101  * @param yaw_rudder Control output -1 .. 1
00102  * @param throttle Throttle 0 .. 1
00103  * @param mode System mode (MAV_MODE)
00104  * @param nav_mode Navigation mode (MAV_NAV_MODE)
00105  */
00106 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00107 
00108 static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, uint8_t mode, uint8_t nav_mode)
00109 {
00110     mavlink_message_t msg;
00111     mavlink_msg_hil_controls_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode);
00112     mavlink_send_uart(chan, &msg);
00113 }
00114 
00115 #endif
00116 // MESSAGE HIL_CONTROLS UNPACKING
00117 
00118 /**
00119  * @brief Get field time_us from hil_controls message
00120  *
00121  * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
00122  */
00123 static inline uint64_t mavlink_msg_hil_controls_get_time_us(const mavlink_message_t* msg)
00124 {
00125     generic_64bit r;
00126     r.b[7] = (msg->payload)[0];
00127     r.b[6] = (msg->payload)[1];
00128     r.b[5] = (msg->payload)[2];
00129     r.b[4] = (msg->payload)[3];
00130     r.b[3] = (msg->payload)[4];
00131     r.b[2] = (msg->payload)[5];
00132     r.b[1] = (msg->payload)[6];
00133     r.b[0] = (msg->payload)[7];
00134     return (uint64_t)r.ll;
00135 }
00136 
00137 /**
00138  * @brief Get field roll_ailerons from hil_controls message
00139  *
00140  * @return Control output -3 .. 1
00141  */
00142 static inline float mavlink_msg_hil_controls_get_roll_ailerons(const mavlink_message_t* msg)
00143 {
00144     generic_32bit r;
00145     r.b[3] = (msg->payload+sizeof(uint64_t))[0];
00146     r.b[2] = (msg->payload+sizeof(uint64_t))[1];
00147     r.b[1] = (msg->payload+sizeof(uint64_t))[2];
00148     r.b[0] = (msg->payload+sizeof(uint64_t))[3];
00149     return (float)r.f;
00150 }
00151 
00152 /**
00153  * @brief Get field pitch_elevator from hil_controls message
00154  *
00155  * @return Control output -1 .. 1
00156  */
00157 static inline float mavlink_msg_hil_controls_get_pitch_elevator(const mavlink_message_t* msg)
00158 {
00159     generic_32bit r;
00160     r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0];
00161     r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1];
00162     r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2];
00163     r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3];
00164     return (float)r.f;
00165 }
00166 
00167 /**
00168  * @brief Get field yaw_rudder from hil_controls message
00169  *
00170  * @return Control output -1 .. 1
00171  */
00172 static inline float mavlink_msg_hil_controls_get_yaw_rudder(const mavlink_message_t* msg)
00173 {
00174     generic_32bit r;
00175     r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0];
00176     r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1];
00177     r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2];
00178     r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3];
00179     return (float)r.f;
00180 }
00181 
00182 /**
00183  * @brief Get field throttle from hil_controls message
00184  *
00185  * @return Throttle 0 .. 1
00186  */
00187 static inline float mavlink_msg_hil_controls_get_throttle(const mavlink_message_t* msg)
00188 {
00189     generic_32bit r;
00190     r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0];
00191     r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1];
00192     r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2];
00193     r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3];
00194     return (float)r.f;
00195 }
00196 
00197 /**
00198  * @brief Get field mode from hil_controls message
00199  *
00200  * @return System mode (MAV_MODE)
00201  */
00202 static inline uint8_t mavlink_msg_hil_controls_get_mode(const mavlink_message_t* msg)
00203 {
00204     return (uint8_t)(msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
00205 }
00206 
00207 /**
00208  * @brief Get field nav_mode from hil_controls message
00209  *
00210  * @return Navigation mode (MAV_NAV_MODE)
00211  */
00212 static inline uint8_t mavlink_msg_hil_controls_get_nav_mode(const mavlink_message_t* msg)
00213 {
00214     return (uint8_t)(msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0];
00215 }
00216 
00217 /**
00218  * @brief Decode a hil_controls message into a struct
00219  *
00220  * @param msg The message to decode
00221  * @param hil_controls C-struct to decode the message contents into
00222  */
00223 static inline void mavlink_msg_hil_controls_decode(const mavlink_message_t* msg, mavlink_hil_controls_t* hil_controls)
00224 {
00225     hil_controls->time_us = mavlink_msg_hil_controls_get_time_us(msg);
00226     hil_controls->roll_ailerons = mavlink_msg_hil_controls_get_roll_ailerons(msg);
00227     hil_controls->pitch_elevator = mavlink_msg_hil_controls_get_pitch_elevator(msg);
00228     hil_controls->yaw_rudder = mavlink_msg_hil_controls_get_yaw_rudder(msg);
00229     hil_controls->throttle = mavlink_msg_hil_controls_get_throttle(msg);
00230     hil_controls->mode = mavlink_msg_hil_controls_get_mode(msg);
00231     hil_controls->nav_mode = mavlink_msg_hil_controls_get_nav_mode(msg);
00232 }