3dr_radio.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/RadioStatus.h>
00021 
00022 namespace mavplugin {
00026 class TDRRadioPlugin : public MavRosPlugin {
00027 public:
00028         TDRRadioPlugin() :
00029                 nh("~"),
00030                 uas(nullptr),
00031                 has_radio_status(false),
00032                 diag_added(false),
00033                 low_rssi(0)
00034         { }
00035 
00036         void initialize(UAS &uas_)
00037         {
00038                 uas = &uas_;
00039 
00040                 nh.param("tdr_radio/low_rssi", low_rssi, 40);
00041 
00042                 status_pub = nh.advertise<mavros_msgs::RadioStatus>("radio_status", 10);
00043 
00044                 uas->sig_connection_changed.connect(boost::bind(&TDRRadioPlugin::connection_cb, this, _1));
00045         }
00046 
00047         const message_map get_rx_handlers() {
00048                 return {
00049                                MESSAGE_HANDLER(MAVLINK_MSG_ID_RADIO_STATUS, &TDRRadioPlugin::handle_radio_status),
00050 #ifdef MAVLINK_MSG_ID_RADIO
00051                                MESSAGE_HANDLER(MAVLINK_MSG_ID_RADIO, &TDRRadioPlugin::handle_radio),
00052 #endif
00053                 };
00054         }
00055 
00056 private:
00057         ros::NodeHandle nh;
00058         UAS *uas;
00059 
00060         bool has_radio_status;
00061         bool diag_added;
00062         int low_rssi;
00063 
00064         ros::Publisher status_pub;
00065 
00066         std::recursive_mutex diag_mutex;
00067         mavros_msgs::RadioStatus::Ptr last_status;
00068 
00069         /* -*- message handlers -*- */
00070 
00071         void handle_radio_status(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00072                 mavlink_radio_status_t rst;
00073                 mavlink_msg_radio_status_decode(msg, &rst);
00074                 has_radio_status = true;
00075                 handle_message(rst, sysid, compid);
00076         }
00077 
00078 #ifdef MAVLINK_MSG_ID_RADIO
00079         void handle_radio(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00080                 if (has_radio_status)
00081                         return;
00082 
00083                 // actually the same data, but from earlier modems
00084                 mavlink_radio_t rst;
00085                 mavlink_msg_radio_decode(msg, &rst);
00086                 handle_message(rst, sysid, compid);
00087         }
00088 #endif
00089 
00090         template<typename msgT>
00091         void handle_message(msgT &rst, uint8_t sysid, uint8_t compid) {
00092                 if (sysid != '3' || compid != 'D')
00093                         ROS_WARN_THROTTLE_NAMED(30, "radio", "RADIO_STATUS not from 3DR modem?");
00094 
00095                 auto msg = boost::make_shared<mavros_msgs::RadioStatus>();
00096 
00097                 msg->header.stamp = ros::Time::now();
00098 
00099 #define RST_COPY(field) msg->field = rst.field
00100                 RST_COPY(rssi);
00101                 RST_COPY(remrssi);
00102                 RST_COPY(txbuf);
00103                 RST_COPY(noise);
00104                 RST_COPY(remnoise);
00105                 RST_COPY(rxerrors);
00106                 RST_COPY(fixed);
00107 #undef RST_COPY
00108 
00109                 // valid for 3DR modem
00110                 msg->rssi_dbm = (rst.rssi / 1.9) - 127;
00111                 msg->remrssi_dbm = (rst.remrssi / 1.9) - 127;
00112 
00113                 // add diag at first event
00114                 if (!diag_added) {
00115                         UAS_DIAG(uas).add("3DR Radio", this, &TDRRadioPlugin::diag_run);
00116                         diag_added = true;
00117                 }
00118 
00119                 // store last status for diag
00120                 {
00121                         lock_guard lock(diag_mutex);
00122                         last_status = msg;
00123                 }
00124 
00125                 status_pub.publish(msg);
00126         }
00127 
00128 
00129         void diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00130                 lock_guard lock(diag_mutex);
00131 
00132                 if (!last_status) {
00133                         stat.summary(2, "No data");
00134                         return;
00135                 }
00136                 else if (last_status->rssi < low_rssi)
00137                         stat.summary(1, "Low RSSI");
00138                 else if (last_status->remrssi < low_rssi)
00139                         stat.summary(1, "Low remote RSSI");
00140                 else
00141                         stat.summary(0, "Normal");
00142 
00143                 stat.addf("RSSI", "%u", last_status->rssi);
00144                 stat.addf("RSSI (dBm)", "%.1f", last_status->rssi_dbm);
00145                 stat.addf("Remote RSSI", "%u", last_status->remrssi);
00146                 stat.addf("Remote RSSI (dBm)", "%.1f", last_status->remrssi_dbm);
00147                 stat.addf("Tx buffer (%)", "%u", last_status->txbuf);
00148                 stat.addf("Noice level", "%u", last_status->noise);
00149                 stat.addf("Remote noice level", "%u", last_status->remnoise);
00150                 stat.addf("Rx errors", "%u", last_status->rxerrors);
00151                 stat.addf("Fixed", "%u", last_status->fixed);
00152         }
00153 
00154         void connection_cb(bool connected) {
00155                 UAS_DIAG(uas).removeByName("3DR Radio");
00156                 diag_added = false;
00157         }
00158 
00159 };
00160 };      // namespace mavplugin
00161 
00162 PLUGINLIB_EXPORT_CLASS(mavplugin::TDRRadioPlugin, mavplugin::MavRosPlugin)
00163 


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