From 21ebcdc0ae8b7b51400f740027c290c1cb7c3442 Mon Sep 17 00:00:00 2001
From: acfloria <florian.achermann@mavt.ethz.ch>
Date: Thu, 5 Jul 2018 10:05:40 +0200
Subject: [PATCH] Publish the satcom subsystem health

---
 msg/subsystem_info.msg                        |  2 +-
 .../telemetry/iridiumsbd/IridiumSBD.cpp       | 24 +++++++++++++++++++
 src/drivers/telemetry/iridiumsbd/IridiumSBD.h |  5 ++++
 3 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/msg/subsystem_info.msg b/msg/subsystem_info.msg
index 56899ae4d8..a08d85abb3 100644
--- a/msg/subsystem_info.msg
+++ b/msg/subsystem_info.msg
@@ -25,7 +25,7 @@ uint64 SUBSYSTEM_TYPE_REVERSEMOTOR = 8388608
 uint64 SUBSYSTEM_TYPE_LOGGING = 16777216
 uint64 SUBSYSTEM_TYPE_SENSORBATTERY = 33554432
 uint64 SUBSYSTEM_TYPE_SENSORPROXIMITY = 67108864
-uint64 SUBSYSTEM_TYPE_MISSION = 134217728
+uint64 SUBSYSTEM_TYPE_SATCOM = 134217728
 
 bool present
 bool enabled
diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp
index 74f73460a2..6730f75d13 100644
--- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp
+++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp
@@ -300,6 +300,7 @@ void IridiumSBD::main_loop(int argc, char *argv[])
 			break;
 		}
 
+		publish_subsystem_status();
 		if (_new_state != _state) {
 			VERBOSE_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[_state], satcom_state_string[_new_state]);
 			_state = _new_state;
@@ -1061,6 +1062,29 @@ void IridiumSBD::publish_iridium_status()
 
 }
 
+void IridiumSBD::publish_subsystem_status()
+{
+	const bool present = true;
+	const bool enabled = true;
+	const bool ok = _status.last_heartbeat > 0; // maybe at some point here an additional check should be made
+
+	if ((_info.present != present) || (_info.enabled != enabled) || (_info.ok != ok)) {
+		_info.timestamp = hrt_absolute_time();
+		_info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_SATCOM;
+		_info.present = present;
+		_info.enabled = enabled;
+		_info.ok = ok;
+
+		if (_subsystem_pub == nullptr) {
+			_subsystem_pub = orb_advertise_queue(ORB_ID(subsystem_info), &_info, subsystem_info_s::ORB_QUEUE_LENGTH);
+
+		} else {
+			orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &_info);
+		}
+	}
+}
+
+
 int	IridiumSBD::open_first(struct file *filep)
 {
 	_cdev_used = true;
diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h
index 3e7eba69e3..98b6c7886c 100644
--- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h
+++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h
@@ -41,6 +41,7 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/iridiumsbd_status.h>
+#include <uORB/topics/subsystem_info.h>
 
 typedef enum {
 	SATCOM_OK = 0,
@@ -256,6 +257,8 @@ private:
 
 	void publish_iridium_status(void);
 
+	void publish_subsystem_status();
+
 	/**
 	 * Notification of the first open of CDev.
 	 *
@@ -301,6 +304,7 @@ private:
 	uint16_t _packet_length = 0;
 
 	orb_advert_t _iridiumsbd_status_pub = nullptr;
+	orb_advert_t _subsystem_pub = nullptr;
 
 	bool _test_pending = false;
 	char _test_command[32];
@@ -338,4 +342,5 @@ private:
 	bool _verbose = false;
 
 	iridiumsbd_status_s _status = {};
+	subsystem_info_s _info = {};
 };