vfr_hud.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2014 Vladimir Ermakov.
00011  *
00012  * This file is part of the mavros package and subject to the license terms
00013  * in the top-level LICENSE file of the mavros repository.
00014  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00015  */
00016 
00017 #include <angles/angles.h>
00018 #include <mavros/mavros_plugin.h>
00019 #include <pluginlib/class_list_macros.h>
00020 
00021 #include <mavros_msgs/VFR_HUD.h>
00022 #include <geometry_msgs/TwistStamped.h>
00023 
00024 namespace mavplugin {
00028 class VfrHudPlugin : public MavRosPlugin {
00029 public:
00030         VfrHudPlugin() :
00031                 nh("~")
00032         { }
00033 
00037         void initialize(UAS &uas)
00038         {
00039                 vfr_pub = nh.advertise<mavros_msgs::VFR_HUD>("vfr_hud", 10);
00040 
00041 #ifdef MAVLINK_MSG_ID_WIND
00042                 wind_pub = nh.advertise<geometry_msgs::TwistStamped>("wind_estimation", 10);
00043 #endif
00044         }
00045 
00046         const message_map get_rx_handlers() {
00047                 return {
00048                                MESSAGE_HANDLER(MAVLINK_MSG_ID_VFR_HUD, &VfrHudPlugin::handle_vfr_hud),
00049 #ifdef MAVLINK_MSG_ID_WIND
00050                                MESSAGE_HANDLER(MAVLINK_MSG_ID_WIND, &VfrHudPlugin::handle_wind),
00051 #endif
00052                 };
00053         }
00054 
00055 private:
00056         ros::NodeHandle nh;
00057 
00058         ros::Publisher vfr_pub;
00059         ros::Publisher wind_pub;
00060 
00061         void handle_vfr_hud(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00062                 mavlink_vfr_hud_t vfr_hud;
00063                 mavlink_msg_vfr_hud_decode(msg, &vfr_hud);
00064 
00065                 auto vmsg = boost::make_shared<mavros_msgs::VFR_HUD>();
00066                 vmsg->header.stamp = ros::Time::now();
00067                 vmsg->airspeed = vfr_hud.airspeed;
00068                 vmsg->groundspeed = vfr_hud.groundspeed;
00069                 vmsg->heading = vfr_hud.heading;
00070                 vmsg->throttle = vfr_hud.throttle / 100.0;      // comes in 0..100 range
00071                 vmsg->altitude = vfr_hud.alt;
00072                 vmsg->climb = vfr_hud.climb;
00073 
00074                 vfr_pub.publish(vmsg);
00075         }
00076 
00077 #ifdef MAVLINK_MSG_ID_WIND
00078 
00081         void handle_wind(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00082                 mavlink_wind_t wind;
00083                 mavlink_msg_wind_decode(msg, &wind);
00084 
00085                 const double speed = wind.speed;
00086                 const double course = angles::from_degrees(wind.direction);
00087 
00088                 auto twist = boost::make_shared<geometry_msgs::TwistStamped>();
00089                 twist->header.stamp = ros::Time::now();
00090                 // TODO: check math's
00091                 twist->twist.linear.x = speed * std::sin(course);
00092                 twist->twist.linear.y = speed * std::cos(course);
00093                 twist->twist.linear.z = wind.speed_z;
00094 
00095                 wind_pub.publish(twist);
00096         }
00097 #endif
00098 };
00099 };      // namespace mavplugin
00100 
00101 PLUGINLIB_EXPORT_CLASS(mavplugin::VfrHudPlugin, mavplugin::MavRosPlugin)
00102 


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19