rc_io.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2014,2015 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 <mavros/mavros_plugin.h>
00018 #include <pluginlib/class_list_macros.h>
00019 
00020 #include <mavros_msgs/RCIn.h>
00021 #include <mavros_msgs/RCOut.h>
00022 #include <mavros_msgs/OverrideRCIn.h>
00023 
00024 namespace mavplugin {
00028 class RCIOPlugin : public MavRosPlugin {
00029 public:
00030         RCIOPlugin() :
00031                 rc_nh("~rc"),
00032                 uas(nullptr),
00033                 raw_rc_in(0),
00034                 raw_rc_out(0),
00035                 has_rc_channels_msg(false)
00036         { };
00037 
00038         void initialize(UAS &uas_)
00039         {
00040                 uas = &uas_;
00041 
00042                 rc_in_pub = rc_nh.advertise<mavros_msgs::RCIn>("in", 10);
00043                 rc_out_pub = rc_nh.advertise<mavros_msgs::RCOut>("out", 10);
00044                 override_sub = rc_nh.subscribe("override", 10, &RCIOPlugin::override_cb, this);
00045 
00046                 uas->sig_connection_changed.connect(boost::bind(&RCIOPlugin::connection_cb, this, _1));
00047         };
00048 
00049         const message_map get_rx_handlers() {
00050                 return {
00051                                MESSAGE_HANDLER(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &RCIOPlugin::handle_rc_channels_raw),
00052                                MESSAGE_HANDLER(MAVLINK_MSG_ID_RC_CHANNELS, &RCIOPlugin::handle_rc_channels),
00053                                MESSAGE_HANDLER(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &RCIOPlugin::handle_servo_output_raw),
00054                 };
00055         }
00056 
00057 private:
00058         std::recursive_mutex mutex;
00059         ros::NodeHandle rc_nh;
00060         UAS *uas;
00061 
00062         std::vector<uint16_t> raw_rc_in;
00063         std::vector<uint16_t> raw_rc_out;
00064         bool has_rc_channels_msg;
00065 
00066         ros::Publisher rc_in_pub;
00067         ros::Publisher rc_out_pub;
00068         ros::Subscriber override_sub;
00069 
00070         /* -*- rx handlers -*- */
00071 
00072         void handle_rc_channels_raw(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00073                 mavlink_rc_channels_raw_t port;
00074                 mavlink_msg_rc_channels_raw_decode(msg, &port);
00075                 lock_guard lock(mutex);
00076 
00077                 /* if we receive RC_CHANNELS, drop RC_CHANNELS_RAW */
00078                 if (has_rc_channels_msg)
00079                         return;
00080 
00081                 size_t offset = port.port * 8;
00082                 if (raw_rc_in.size() < offset + 8)
00083                         raw_rc_in.resize(offset + 8);
00084 
00085 #define SET_RC_IN(mavidx)       \
00086         raw_rc_in[offset + mavidx - 1] = port.chan ## mavidx ## _raw
00087                 SET_RC_IN(1);
00088                 SET_RC_IN(2);
00089                 SET_RC_IN(3);
00090                 SET_RC_IN(4);
00091                 SET_RC_IN(5);
00092                 SET_RC_IN(6);
00093                 SET_RC_IN(7);
00094                 SET_RC_IN(8);
00095 #undef SET_RC_IN
00096 
00097                 auto rcin_msg = boost::make_shared<mavros_msgs::RCIn>();
00098 
00099                 rcin_msg->header.stamp = uas->synchronise_stamp(port.time_boot_ms);
00100                 rcin_msg->rssi = port.rssi;
00101                 rcin_msg->channels = raw_rc_in;
00102 
00103                 rc_in_pub.publish(rcin_msg);
00104         }
00105 
00106         void handle_rc_channels(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00107                 mavlink_rc_channels_t channels;
00108                 mavlink_msg_rc_channels_decode(msg, &channels);
00109                 lock_guard lock(mutex);
00110 
00111                 ROS_INFO_COND_NAMED(!has_rc_channels_msg, "rc", "RC_CHANNELS message detected!");
00112                 has_rc_channels_msg = true;
00113 
00114                 if (channels.chancount > 18) {
00115                         raw_rc_in.resize(18);
00116                         ROS_WARN_THROTTLE_NAMED(60, "rc",
00117                                         "FCU receives %u RC channels, but RC_CHANNELS can store 18",
00118                                         channels.chancount);
00119                 }
00120                 else
00121                         raw_rc_in.resize(channels.chancount);
00122 
00123 #define IFSET_RC_IN(mavidx)                             \
00124         if (channels.chancount >= mavidx)       \
00125                 raw_rc_in[mavidx - 1] = channels.chan ## mavidx ## _raw
00126                 IFSET_RC_IN(1);
00127                 IFSET_RC_IN(2);
00128                 IFSET_RC_IN(3);
00129                 IFSET_RC_IN(4);
00130                 IFSET_RC_IN(5);
00131                 IFSET_RC_IN(6);
00132                 IFSET_RC_IN(7);
00133                 IFSET_RC_IN(8);
00134                 IFSET_RC_IN(9);
00135                 IFSET_RC_IN(10);
00136                 IFSET_RC_IN(11);
00137                 IFSET_RC_IN(12);
00138                 IFSET_RC_IN(13);
00139                 IFSET_RC_IN(14);
00140                 IFSET_RC_IN(15);
00141                 IFSET_RC_IN(16);
00142                 IFSET_RC_IN(17);
00143                 IFSET_RC_IN(18);
00144 #undef IFSET_RC_IN
00145 
00146                 auto rcin_msg = boost::make_shared<mavros_msgs::RCIn>();
00147 
00148                 rcin_msg->header.stamp = uas->synchronise_stamp(channels.time_boot_ms);
00149                 rcin_msg->rssi = channels.rssi;
00150                 rcin_msg->channels = raw_rc_in;
00151 
00152                 rc_in_pub.publish(rcin_msg);
00153         }
00154 
00155         void handle_servo_output_raw(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00156                 mavlink_servo_output_raw_t port;
00157                 mavlink_msg_servo_output_raw_decode(msg, &port);
00158                 lock_guard lock(mutex);
00159 
00160                 size_t offset = port.port * 8;
00161                 if (raw_rc_out.size() < offset + 8)
00162                         raw_rc_out.resize(offset + 8);
00163 
00164 #define SET_RC_OUT(mavidx)      \
00165         raw_rc_out[offset + mavidx - 1] = port.servo ## mavidx ## _raw
00166                 SET_RC_OUT(1);
00167                 SET_RC_OUT(2);
00168                 SET_RC_OUT(3);
00169                 SET_RC_OUT(4);
00170                 SET_RC_OUT(5);
00171                 SET_RC_OUT(6);
00172                 SET_RC_OUT(7);
00173                 SET_RC_OUT(8);
00174 #undef SET_RC_OUT
00175 
00176                 auto rcout_msg = boost::make_shared<mavros_msgs::RCOut>();
00177 
00178                 // XXX: Why time_usec id 32 bit? We should test that.
00179                 uint64_t time_usec = port.time_usec;
00180 
00181                 rcout_msg->header.stamp = uas->synchronise_stamp(time_usec);
00182                 rcout_msg->channels = raw_rc_out;
00183 
00184                 rc_out_pub.publish(rcout_msg);
00185         }
00186 
00187         /* -*- low-level send functions -*- */
00188 
00189         void rc_channels_override(const boost::array<uint16_t, 8> &channels) {
00190                 mavlink_message_t msg;
00191 
00192                 mavlink_msg_rc_channels_override_pack_chan(UAS_PACK_CHAN(uas), &msg,
00193                                 UAS_PACK_TGT(uas),
00194                                 channels[0],
00195                                 channels[1],
00196                                 channels[2],
00197                                 channels[3],
00198                                 channels[4],
00199                                 channels[5],
00200                                 channels[6],
00201                                 channels[7]
00202                                 );
00203                 UAS_FCU(uas)->send_message(&msg);
00204         }
00205 
00206         /* -*- callbacks -*- */
00207 
00208         void connection_cb(bool connected) {
00209                 lock_guard lock(mutex);
00210                 raw_rc_in.clear();
00211                 raw_rc_out.clear();
00212                 has_rc_channels_msg = false;
00213         }
00214 
00215         void override_cb(const mavros_msgs::OverrideRCIn::ConstPtr req) {
00216                 if (!uas->is_ardupilotmega() && !uas->is_px4())
00217                         ROS_WARN_THROTTLE_NAMED(30, "rc", "RC override not supported by this FCU!");
00218 
00219                 rc_channels_override(req->channels);
00220         }
00221 };
00222 };      // namespace mavplugin
00223 
00224 PLUGINLIB_EXPORT_CLASS(mavplugin::RCIOPlugin, mavplugin::MavRosPlugin)
00225 


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