00001
00009
00010
00011
00012
00013
00014
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
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
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
00110 msg->rssi_dbm = (rst.rssi / 1.9) - 127;
00111 msg->remrssi_dbm = (rst.remrssi / 1.9) - 127;
00112
00113
00114 if (!diag_added) {
00115 UAS_DIAG(uas).add("3DR Radio", this, &TDRRadioPlugin::diag_run);
00116 diag_added = true;
00117 }
00118
00119
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 };
00161
00162 PLUGINLIB_EXPORT_CLASS(mavplugin::TDRRadioPlugin, mavplugin::MavRosPlugin)
00163