IridiumSBD: Add iridiumsbd_status uorb message

msg/CMakeLists.txt

@@ -62,6 +62,7 @@ set(msg_files
 	gps_inject_data.msg
 	home_position.msg
 	input_rc.msg
+	iridiumsbd_status.msg
 	irlock_report.msg
 	landing_target_innovations.msg
 	landing_target_pose.msg

msg/iridiumsbd_status.msg

@@ -0,0 +1,14 @@
+uint64 last_heartbeat				# timestamp of the last successful sbd session
+uint16 tx_buf_write_index			# current size of the tx buffer
+uint16 rx_buf_read_index			# the rx buffer is parsed up to that index
+uint16 rx_buf_end_index				# current size of the rx buffer
+uint16 failed_sbd_sessions			# number of failed sbd sessions
+uint16 successful_sbd_sessions      # number of successfull sbd sessions
+uint16 num_tx_buf_reset             # number of times the tx buffer was reset
+uint8 signal_quality                # current signal quality, 0 is no signal, 5 the best
+uint8 state                         # current state of the driver, see the satcom_state of IridiumSBD.h for the definition
+bool ring_pending                   # indicates if a ring call is pending
+bool tx_buf_write_pending           # indicates if a tx buffer write is pending
+bool tx_session_pending             # indicates if a tx session is pending
+bool rx_read_pending                # indicates if a rx read is pending
+bool rx_session_pending             # indicates if a rx session is pending

src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp

@@ -303,8 +303,10 @@ void IridiumSBD::main_loop(int argc, char *argv[])
 		if (_new_state != _state) {
 			VERBOSE_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[_state], satcom_state_string[_new_state]);
 			_state = _new_state;
+			publish_iridium_status();
 
 		} else {
+			publish_iridium_status();
 			usleep(100000);	// 100ms
 		}
 	}
@@ -474,6 +476,8 @@ void IridiumSBD::sbdsession_loop(void)
 		_tx_session_pending = false;
 		_last_read_time = hrt_absolute_time();
 		_last_heartbeat = _last_read_time;
+		++_successful_sbd_sessions;
+
 
 		if (mt_len > 0) {
 			_rx_read_pending = true;
@@ -489,6 +493,7 @@ void IridiumSBD::sbdsession_loop(void)
 
 		// after a successful session reset the tx buffer
 		_tx_buf_write_idx = 0;
+		++_successful_sbd_sessions;
 
 		_tx_session_pending = false;
 		break;
@@ -655,6 +660,7 @@ ssize_t IridiumSBD::write(struct file *filp, const char *buffer, size_t buflen)
 	// check if there is enough space to write the message
 	if (SATCOM_TX_BUF_LEN - _tx_buf_write_idx - _packet_length < 0) {
 		_tx_buf_write_idx = 0;
+		++_num_tx_buf_reset;
 		publish_telemetry_status();
 	}
 
@@ -974,6 +980,94 @@ void IridiumSBD::publish_telemetry_status()
 	}
 }
 
+void IridiumSBD::publish_iridium_status()
+{
+	bool need_to_publish = false;
+
+	if (_status.last_heartbeat != _last_heartbeat) {
+		need_to_publish = true;
+		_status.last_heartbeat = _last_heartbeat;
+	}
+
+	if (_status.tx_buf_write_index != _tx_buf_write_idx) {
+		need_to_publish = true;
+		_status.tx_buf_write_index = _tx_buf_write_idx;
+	}
+
+	if (_status.rx_buf_read_index != _rx_msg_read_idx) {
+		need_to_publish = true;
+		_status.rx_buf_read_index = _rx_msg_read_idx;
+	}
+
+	if (_status.rx_buf_end_index != _rx_msg_end_idx) {
+		need_to_publish = true;
+		_status.rx_buf_end_index = _rx_msg_end_idx;
+	}
+
+	if (_status.failed_sbd_sessions != _failed_sbd_sessions) {
+		need_to_publish = true;
+		_status.failed_sbd_sessions = _failed_sbd_sessions;
+	}
+
+	if (_status.successful_sbd_sessions != _successful_sbd_sessions) {
+		need_to_publish = true;
+		_status.successful_sbd_sessions = _successful_sbd_sessions;
+	}
+
+	if (_status.num_tx_buf_reset != _num_tx_buf_reset) {
+		need_to_publish = true;
+		_status.num_tx_buf_reset = _num_tx_buf_reset;
+	}
+
+	if (_status.signal_quality != _signal_quality) {
+		need_to_publish = true;
+		_status.signal_quality = _signal_quality;
+	}
+
+	if (_status.state != _state) {
+		need_to_publish = true;
+		_status.state = _state;
+	}
+
+	if (_status.ring_pending != _ring_pending) {
+		need_to_publish = true;
+		_status.ring_pending = _ring_pending;
+	}
+
+	if (_status.tx_buf_write_pending != _tx_buf_write_pending) {
+		need_to_publish = true;
+		_status.tx_buf_write_pending = _tx_buf_write_pending;
+	}
+
+	if (_status.tx_session_pending != _tx_session_pending) {
+		need_to_publish = true;
+		_status.tx_session_pending = _tx_session_pending;
+	}
+
+	if (_status.rx_read_pending != _rx_read_pending) {
+		need_to_publish = true;
+		_status.rx_read_pending = _rx_read_pending;
+	}
+
+	if (_status.rx_session_pending != _rx_session_pending) {
+		need_to_publish = true;
+		_status.rx_session_pending = _rx_session_pending;
+	}
+
+	_status.timestamp = hrt_absolute_time();
+
+	// publish the status if it changed
+	if (need_to_publish) {
+		if (_iridiumsbd_status_pub == nullptr) {
+			_iridiumsbd_status_pub = orb_advertise(ORB_ID(iridiumsbd_status), &_status);
+
+		} else {
+			orb_publish(ORB_ID(iridiumsbd_status), _iridiumsbd_status_pub, &_status);
+		}
+	}
+
+}
+
 int	IridiumSBD::open_first(struct file *filep)
 {
 	_cdev_used = true;

src/drivers/telemetry/iridiumsbd/IridiumSBD.h

@@ -41,6 +41,7 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/iridiumsbd_status.h>
 
 typedef enum {
 	SATCOM_OK = 0,
@@ -259,6 +260,8 @@ private:
 	 */
 	void publish_telemetry_status(void);
 
+	void publish_iridium_status(void);
+
 	/**
 	 * Notification of the first open of CDev.
 	 *
@@ -297,11 +300,14 @@ private:
 	hrt_abstime _last_signal_check = 0;
 	uint8_t _signal_quality = 0;
 	uint16_t _failed_sbd_sessions = 0;
+	uint16_t _successful_sbd_sessions = 0;
+	uint16_t _num_tx_buf_reset = 0;
 
 	bool _writing_mavlink_packet = false;
 	uint16_t _packet_length = 0;
 
 	orb_advert_t _telemetry_status_pub = nullptr;
+	orb_advert_t _iridiumsbd_status_pub = nullptr;
 
 	bool _test_pending = false;
 	char _test_command[32];
@@ -335,4 +341,6 @@ private:
 
 	pthread_mutex_t _tx_buf_mutex = pthread_mutex_t();
 	bool _verbose = false;
+
+	iridiumsbd_status_s _status = {};
 };