From 415786a48046f7fd40ea1de757043a8c323227c6 Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Fri, 10 Dec 2010 07:26:59 +0000 Subject: [PATCH] Added airspeed message to mavlink. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1102 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/GCS_MAVLink/include/common/common.h | 3 +- .../GCS_MAVLink/include/common/mavlink.h | 2 +- .../include/common/mavlink_msg_airspeed.h | 74 +++++++++++++++++++ .../message_definitions/common.xml | 5 ++ 4 files changed, 82 insertions(+), 2 deletions(-) create mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_airspeed.h diff --git a/libraries/GCS_MAVLink/include/common/common.h b/libraries/GCS_MAVLink/include/common/common.h index 41cba5f284..ffb06029f9 100644 --- a/libraries/GCS_MAVLink/include/common/common.h +++ b/libraries/GCS_MAVLink/include/common/common.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, December 7 2010, 13:34 UTC + * Generated on Friday, December 10 2010, 07:24 UTC */ #ifndef COMMON_H #define COMMON_H @@ -65,6 +65,7 @@ extern "C" { #include "./mavlink_msg_manual_control.h" #include "./mavlink_msg_debug_vect.h" #include "./mavlink_msg_gps_local_origin_set.h" +#include "./mavlink_msg_airspeed.h" #include "./mavlink_msg_statustext.h" #include "./mavlink_msg_debug.h" #ifdef __cplusplus diff --git a/libraries/GCS_MAVLink/include/common/mavlink.h b/libraries/GCS_MAVLink/include/common/mavlink.h index a5c849425e..0865356696 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink.h +++ b/libraries/GCS_MAVLink/include/common/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, December 7 2010, 13:34 UTC + * Generated on Friday, December 10 2010, 07:24 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_airspeed.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_airspeed.h new file mode 100644 index 0000000000..c77b6c1582 --- /dev/null +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_airspeed.h @@ -0,0 +1,74 @@ +// MESSAGE AIRSPEED PACKING + +#define MAVLINK_MSG_ID_AIRSPEED 72 + +typedef struct __mavlink_airspeed_t +{ + float airspeed; ///< meters/second + +} mavlink_airspeed_t; + + + +/** + * @brief Send a airspeed message + * + * @param airspeed meters/second + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_airspeed_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float airspeed) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_AIRSPEED; + + i += put_float_by_index(airspeed, i, msg->payload); //meters/second + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +static inline uint16_t mavlink_msg_airspeed_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float airspeed) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_AIRSPEED; + + i += put_float_by_index(airspeed, i, msg->payload); //meters/second + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +static inline uint16_t mavlink_msg_airspeed_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_airspeed_t* airspeed) +{ + return mavlink_msg_airspeed_pack(system_id, component_id, msg, airspeed->airspeed); +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_airspeed_send(mavlink_channel_t chan, float airspeed) +{ + mavlink_message_t msg; + mavlink_msg_airspeed_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, airspeed); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE AIRSPEED UNPACKING + +/** + * @brief Get field airspeed from airspeed message + * + * @return meters/second + */ +static inline float mavlink_msg_airspeed_get_airspeed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload)[0]; + r.b[2] = (msg->payload)[1]; + r.b[1] = (msg->payload)[2]; + r.b[0] = (msg->payload)[3]; + return (float)r.f; +} + +static inline void mavlink_msg_airspeed_decode(const mavlink_message_t* msg, mavlink_airspeed_t* airspeed) +{ + airspeed->airspeed = mavlink_msg_airspeed_get_airspeed(msg); +} diff --git a/libraries/GCS_MAVLink/message_definitions/common.xml b/libraries/GCS_MAVLink/message_definitions/common.xml index f5f8128c34..32121d566a 100644 --- a/libraries/GCS_MAVLink/message_definitions/common.xml +++ b/libraries/GCS_MAVLink/message_definitions/common.xml @@ -409,6 +409,11 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se Local Y coordinate in meters Local Z coordinate in meters + + + The calculated airspeed + meters/second +