plugin: 3dr_radio #248: add/remove diag conditionally

mavlink · Mar 11, 2015 · 84fe2f2 · 84fe2f2
1 parent 896bd80
commit 84fe2f2
Show file tree

Hide file tree

Showing 2 changed files with 72 additions and 71 deletions.
diff --git a/mavros/msg/RadioStatus.msg b/mavros/msg/RadioStatus.msg
@@ -1,10 +1,16 @@
 # RADIO_STATUS message
 
 Header header
+
+# message data
 uint8 rssi
 uint8 remrssi
 uint8 txbuf
 uint8 noise
 uint8 remnoise
 uint16 rxerrors
 uint16 fixed
+
+# calculated
+float32 rssi_dbm
+float32 remrssi_dbm
diff --git a/mavros/src/plugins/3dr_radio.cpp b/mavros/src/plugins/3dr_radio.cpp
@@ -20,84 +20,26 @@
 #include <mavros/RadioStatus.h>
 
 namespace mavplugin {
-/**
- * @brief 3DR diagnostics
- */
-class TDRRadioStatus : public diagnostic_updater::DiagnosticTask
-{
-public:
-	TDRRadioStatus(const std::string name, uint8_t _low_rssi) :
-		diagnostic_updater::DiagnosticTask(name),
-		data_received(false),
-		low_rssi(_low_rssi),
-		last_rst {}
-	{ }
-
-
-	template <typename msgT>
-	void set(msgT &rst) {
-		lock_guard lock(mutex);
-		data_received = true;
-#define RST_COPY(field)	last_rst.field = rst.field
-		RST_COPY(rssi);
-		RST_COPY(remrssi);
-		RST_COPY(txbuf);
-		RST_COPY(noise);
-		RST_COPY(remnoise);
-		RST_COPY(rxerrors);
-		RST_COPY(fixed);
-#undef RST_COPY
-	}
-
-	void run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
-		lock_guard lock(mutex);
-
-		if (!data_received)
-			stat.summary(2, "No data");
-		else if (last_rst.rssi < low_rssi)
-			stat.summary(1, "Low RSSI");
-		else if (last_rst.remrssi < low_rssi)
-			stat.summary(1, "Low remote RSSI");
-		else
-			stat.summary(0, "Normal");
-
-		float rssi_dbm = (last_rst.rssi / 1.9) - 127;
-		float remrssi_dbm = (last_rst.remrssi / 1.9) - 127;
-
-		stat.addf("RSSI", "%u", last_rst.rssi);
-		stat.addf("RSSI (dBm)", "%.1f", rssi_dbm);
-		stat.addf("Remote RSSI", "%u", last_rst.remrssi);
-		stat.addf("Remote RSSI (dBm)", "%.1f", remrssi_dbm);
-		stat.addf("Tx buffer (%)", "%u", last_rst.txbuf);
-		stat.addf("Noice level", "%u", last_rst.noise);
-		stat.addf("Remote noice level", "%u", last_rst.remnoise);
-		stat.addf("Rx errors", "%u", last_rst.rxerrors);
-		stat.addf("Fixed", "%u", last_rst.fixed);
-	}
-
-private:
-	std::recursive_mutex mutex;
-	mavlink_radio_status_t last_rst;
-	bool data_received;
-	const uint8_t low_rssi;
-};
-
-
 /**
  * @brief 3DR Radio plugin.
  */
 class TDRRadioPlugin : public MavRosPlugin {
 public:
 	TDRRadioPlugin() :
 		nh("~"),
-		tdr_diag("3DR Radio", 40),
-		has_radio_status(false)
+		has_radio_status(false),
+		diag_added(false)
 	{ }
 
-	void initialize(UAS &uas)
+	void initialize(UAS &uas_)
 	{
-		UAS_DIAG(&uas).add(tdr_diag);
+		uas = &uas_;
+
+		nh.param("tdr_radio/low_rssi", low_rssi, 40);
+
 		status_pub = nh.advertise<mavros::RadioStatus>("radio_status", 10);
+
+		uas->sig_connection_changed.connect(boost::bind(&TDRRadioPlugin::connection_cb, this, _1));
 	}
 
 	const message_map get_rx_handlers() {
@@ -111,11 +53,17 @@ class TDRRadioPlugin : public MavRosPlugin {
 
 private:
 	ros::NodeHandle nh;
-	TDRRadioStatus tdr_diag;
+	UAS *uas;
+
 	bool has_radio_status;
+	bool diag_added;
+	int low_rssi;
 
 	ros::Publisher status_pub;
 
+	std::recursive_mutex diag_mutex;
+	mavros::RadioStatus::Ptr last_status;
+
 	/* -*- message handlers -*- */
 
 	void handle_radio_status(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
@@ -142,9 +90,9 @@ class TDRRadioPlugin : public MavRosPlugin {
 		if (sysid != '3' || compid != 'D')
 			ROS_WARN_THROTTLE_NAMED(30, "radio", "RADIO_STATUS not from 3DR modem?");
 
-		tdr_diag.set(rst);
+		auto msg = boost::make_shared<mavros::RadioStatus>();
 
-		mavros::RadioStatusPtr msg = boost::make_shared<mavros::RadioStatus>();
+		msg->header.stamp = ros::Time::now();
 
 #define RST_COPY(field)	msg->field = rst.field
 		RST_COPY(rssi);
@@ -156,9 +104,56 @@ class TDRRadioPlugin : public MavRosPlugin {
 		RST_COPY(fixed);
 #undef RST_COPY
 
-		msg->header.stamp = ros::Time::now();
+		// valid for 3DR modem
+		msg->rssi_dbm = (rst.rssi / 1.9) - 127;
+		msg->remrssi_dbm = (rst.remrssi / 1.9) - 127;
+
+		// add diag at first event
+		if (!diag_added) {
+			UAS_DIAG(uas).add("3DR Radio", this, &TDRRadioPlugin::diag_run);
+			diag_added = true;
+		}
+
+		// store last status for diag
+		{
+			lock_guard lock(diag_mutex);
+			last_status = msg;
+		}
+
 		status_pub.publish(msg);
 	}
+
+
+	void diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
+		lock_guard lock(diag_mutex);
+
+		if (!last_status) {
+			stat.summary(2, "No data");
+			return;
+		}
+		else if (last_status->rssi < low_rssi)
+			stat.summary(1, "Low RSSI");
+		else if (last_status->remrssi < low_rssi)
+			stat.summary(1, "Low remote RSSI");
+		else
+			stat.summary(0, "Normal");
+
+		stat.addf("RSSI", "%u", last_status->rssi);
+		stat.addf("RSSI (dBm)", "%.1f", last_status->rssi_dbm);
+		stat.addf("Remote RSSI", "%u", last_status->remrssi);
+		stat.addf("Remote RSSI (dBm)", "%.1f", last_status->remrssi_dbm);
+		stat.addf("Tx buffer (%)", "%u", last_status->txbuf);
+		stat.addf("Noice level", "%u", last_status->noise);
+		stat.addf("Remote noice level", "%u", last_status->remnoise);
+		stat.addf("Rx errors", "%u", last_status->rxerrors);
+		stat.addf("Fixed", "%u", last_status->fixed);
+	}
+
+	void connection_cb(bool connected) {
+		UAS_DIAG(uas).removeByName("3DR Radio");
+		diag_added = false;
+	}
+
 };
 };	// namespace mavplugin