diff --git a/mavros/CMakeLists.txt b/mavros/CMakeLists.txt
index ceeb2fd58..bfef5e118 100644
--- a/mavros/CMakeLists.txt
+++ b/mavros/CMakeLists.txt
@@ -67,6 +67,7 @@ add_message_files(
RCOut.msg
RadioStatus.msg
OverrideRCIn.msg
+ VFR_HUD.msg
)
## Generate services in the 'srv' folder
@@ -185,6 +186,7 @@ add_library(mavros_plugins
src/plugins/setpoint_velocity.cpp
src/plugins/setpoint_accel.cpp
src/plugins/setpoint_attitude.cpp
+ src/plugins/vfr_hud.cpp
)
add_dependencies(mavros_plugins
mavros
diff --git a/mavros/mavros_plugins.xml b/mavros/mavros_plugins.xml
index f8f51f09e..c440422c7 100644
--- a/mavros/mavros_plugins.xml
+++ b/mavros/mavros_plugins.xml
@@ -47,5 +47,8 @@
Send to FCU external attitude setpoint.
+
+ Publish VFR HUD data and WIND estimations.
+
diff --git a/mavros/msg/VFR_HUD.msg b/mavros/msg/VFR_HUD.msg
new file mode 100644
index 000000000..c34204ab6
--- /dev/null
+++ b/mavros/msg/VFR_HUD.msg
@@ -0,0 +1,11 @@
+# Metrics typically displayed on a HUD for fixed wing aircraft
+#
+# VFR_HUD message
+
+Header header
+float32 airspeed # m/s
+float32 groundspeed # m/s
+int16 heading # degrees 0..360
+float32 throttle # normalized to 0.0..1.0
+float32 altitude # MSL
+float32 climb # current climb rate m/s
diff --git a/mavros/src/plugins/vfr_hud.cpp b/mavros/src/plugins/vfr_hud.cpp
new file mode 100644
index 000000000..2534012af
--- /dev/null
+++ b/mavros/src/plugins/vfr_hud.cpp
@@ -0,0 +1,117 @@
+/**
+ * @brief VFR HUD plugin
+ * @file vfr_hud.cpp
+ * @author Vladimir Ermakov
+ *
+ * @addtogroup plugin
+ * @{
+ */
+/*
+ * Copyright 2014 Vladimir Ermakov.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include
+#include
+#include
+
+#include
+#include
+
+namespace mavplugin {
+
+/**
+ * @brief VFR HUD plugin.
+ */
+class VfrHudPlugin : public MavRosPlugin {
+public:
+ VfrHudPlugin()
+ { }
+
+ /**
+ * Plugin initializer. Constructor should not do this.
+ */
+ void initialize(UAS &uas,
+ ros::NodeHandle &nh,
+ diagnostic_updater::Updater &diag_updater)
+ {
+ vfr_pub = nh.advertise("vfr_hud", 10);
+
+#ifdef MAVLINK_MSG_ID_WIND
+ wind_pub = nh.advertise("wind_estimation", 10);
+#endif
+ }
+
+ std::string const get_name() const {
+ return "VFRHUD";
+ }
+
+ const message_map get_rx_handlers() {
+ return {
+ MESSAGE_HANDLER(MAVLINK_MSG_ID_VFR_HUD, &VfrHudPlugin::handle_vfr_hud),
+#ifdef MAVLINK_MSG_ID_WIND
+ MESSAGE_HANDLER(MAVLINK_MSG_ID_WIND, &VfrHudPlugin::handle_wind),
+#endif
+ };
+ }
+
+private:
+ ros::Publisher vfr_pub;
+ ros::Publisher wind_pub;
+
+ void handle_vfr_hud(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
+ mavlink_vfr_hud_t vfr_hud;
+ mavlink_msg_vfr_hud_decode(msg, &vfr_hud);
+
+ auto vmsg = boost::make_shared();
+ vmsg->header.stamp = ros::Time::now();
+ vmsg->airspeed = vfr_hud.airspeed;
+ vmsg->groundspeed = vfr_hud.groundspeed;
+ vmsg->heading = vfr_hud.heading;
+ vmsg->throttle = vfr_hud.throttle / 100.0; // comes in 0..100 range
+ vmsg->altitude = vfr_hud.alt;
+ vmsg->climb = vfr_hud.climb;
+
+ vfr_pub.publish(vmsg);
+ }
+
+#ifdef MAVLINK_MSG_ID_WIND
+ /**
+ * Handle APM specific wind direction estimation message
+ */
+ void handle_wind(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
+ mavlink_wind_t wind;
+ mavlink_msg_wind_decode(msg, &wind);
+
+ const double speed = wind.speed;
+ const double course = angles::from_degrees(wind.direction);
+
+ auto twist = boost::make_shared();
+ twist->header.stamp = ros::Time::now();
+ // TODO: check math's
+ twist->twist.linear.x = speed * std::sin(course);
+ twist->twist.linear.y = speed * std::cos(course);
+ twist->twist.linear.z = wind.speed_z;
+
+ wind_pub.publish(twist);
+ }
+#endif
+};
+
+}; // namespace mavplugin
+
+PLUGINLIB_EXPORT_CLASS(mavplugin::VfrHudPlugin, mavplugin::MavRosPlugin)
+