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

mavlink_msg_air_data.h

00001 // MESSAGE AIR_DATA PACKING
00002 
00003 #define MAVLINK_MSG_ID_AIR_DATA 171
00004 
00005 typedef struct __mavlink_air_data_t 
00006 {
00007     float dynamicPressure; ///< Dynamic pressure (Pa)
00008     float staticPressure; ///< Static pressure (Pa)
00009     uint16_t temperature; ///< Board temperature
00010 
00011 } mavlink_air_data_t;
00012 
00013 
00014 
00015 /**
00016  * @brief Pack a air_data message
00017  * @param system_id ID of this system
00018  * @param component_id ID of this component (e.g. 200 for IMU)
00019  * @param msg The MAVLink message to compress the data into
00020  *
00021  * @param dynamicPressure Dynamic pressure (Pa)
00022  * @param staticPressure Static pressure (Pa)
00023  * @param temperature Board temperature
00024  * @return length of the message in bytes (excluding serial stream start sign)
00025  */
00026 static inline uint16_t mavlink_msg_air_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float dynamicPressure, float staticPressure, uint16_t temperature)
00027 {
00028     uint16_t i = 0;
00029     msg->msgid = MAVLINK_MSG_ID_AIR_DATA;
00030 
00031     i += put_float_by_index(dynamicPressure, i, msg->payload); // Dynamic pressure (Pa)
00032     i += put_float_by_index(staticPressure, i, msg->payload); // Static pressure (Pa)
00033     i += put_uint16_t_by_index(temperature, i, msg->payload); // Board temperature
00034 
00035     return mavlink_finalize_message(msg, system_id, component_id, i);
00036 }
00037 
00038 /**
00039  * @brief Pack a air_data message
00040  * @param system_id ID of this system
00041  * @param component_id ID of this component (e.g. 200 for IMU)
00042  * @param chan The MAVLink channel this message was sent over
00043  * @param msg The MAVLink message to compress the data into
00044  * @param dynamicPressure Dynamic pressure (Pa)
00045  * @param staticPressure Static pressure (Pa)
00046  * @param temperature Board temperature
00047  * @return length of the message in bytes (excluding serial stream start sign)
00048  */
00049 static inline uint16_t mavlink_msg_air_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float dynamicPressure, float staticPressure, uint16_t temperature)
00050 {
00051     uint16_t i = 0;
00052     msg->msgid = MAVLINK_MSG_ID_AIR_DATA;
00053 
00054     i += put_float_by_index(dynamicPressure, i, msg->payload); // Dynamic pressure (Pa)
00055     i += put_float_by_index(staticPressure, i, msg->payload); // Static pressure (Pa)
00056     i += put_uint16_t_by_index(temperature, i, msg->payload); // Board temperature
00057 
00058     return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
00059 }
00060 
00061 /**
00062  * @brief Encode a air_data struct into a message
00063  *
00064  * @param system_id ID of this system
00065  * @param component_id ID of this component (e.g. 200 for IMU)
00066  * @param msg The MAVLink message to compress the data into
00067  * @param air_data C-struct to read the message contents from
00068  */
00069 static inline uint16_t mavlink_msg_air_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_air_data_t* air_data)
00070 {
00071     return mavlink_msg_air_data_pack(system_id, component_id, msg, air_data->dynamicPressure, air_data->staticPressure, air_data->temperature);
00072 }
00073 
00074 /**
00075  * @brief Send a air_data message
00076  * @param chan MAVLink channel to send the message
00077  *
00078  * @param dynamicPressure Dynamic pressure (Pa)
00079  * @param staticPressure Static pressure (Pa)
00080  * @param temperature Board temperature
00081  */
00082 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
00083 
00084 static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynamicPressure, float staticPressure, uint16_t temperature)
00085 {
00086     mavlink_message_t msg;
00087     mavlink_msg_air_data_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, dynamicPressure, staticPressure, temperature);
00088     mavlink_send_uart(chan, &msg);
00089 }
00090 
00091 #endif
00092 // MESSAGE AIR_DATA UNPACKING
00093 
00094 /**
00095  * @brief Get field dynamicPressure from air_data message
00096  *
00097  * @return Dynamic pressure (Pa)
00098  */
00099 static inline float mavlink_msg_air_data_get_dynamicPressure(const mavlink_message_t* msg)
00100 {
00101     generic_32bit r;
00102     r.b[3] = (msg->payload)[0];
00103     r.b[2] = (msg->payload)[1];
00104     r.b[1] = (msg->payload)[2];
00105     r.b[0] = (msg->payload)[3];
00106     return (float)r.f;
00107 }
00108 
00109 /**
00110  * @brief Get field staticPressure from air_data message
00111  *
00112  * @return Static pressure (Pa)
00113  */
00114 static inline float mavlink_msg_air_data_get_staticPressure(const mavlink_message_t* msg)
00115 {
00116     generic_32bit r;
00117     r.b[3] = (msg->payload+sizeof(float))[0];
00118     r.b[2] = (msg->payload+sizeof(float))[1];
00119     r.b[1] = (msg->payload+sizeof(float))[2];
00120     r.b[0] = (msg->payload+sizeof(float))[3];
00121     return (float)r.f;
00122 }
00123 
00124 /**
00125  * @brief Get field temperature from air_data message
00126  *
00127  * @return Board temperature
00128  */
00129 static inline uint16_t mavlink_msg_air_data_get_temperature(const mavlink_message_t* msg)
00130 {
00131     generic_16bit r;
00132     r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[0];
00133     r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[1];
00134     return (uint16_t)r.s;
00135 }
00136 
00137 /**
00138  * @brief Decode a air_data message into a struct
00139  *
00140  * @param msg The message to decode
00141  * @param air_data C-struct to decode the message contents into
00142  */
00143 static inline void mavlink_msg_air_data_decode(const mavlink_message_t* msg, mavlink_air_data_t* air_data)
00144 {
00145     air_data->dynamicPressure = mavlink_msg_air_data_get_dynamicPressure(msg);
00146     air_data->staticPressure = mavlink_msg_air_data_get_staticPressure(msg);
00147     air_data->temperature = mavlink_msg_air_data_get_temperature(msg);
00148 }