Pr iridium mode v2

msg/IridiumsbdStatus.msg

@@ -1,5 +1,5 @@
 uint64 timestamp				# time since system start (microseconds)
-uint64 last_heartbeat				# timestamp of the last successful sbd session
+uint64 last_at_ok_timestamp			# timestamp of the last "OK" received after the "AT" command
 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

src/drivers/telemetry/CMakeLists.txt

@@ -34,4 +34,4 @@
 add_subdirectory(bst)
 add_subdirectory(frsky_telemetry)
 add_subdirectory(hott)
-#add_subdirectory(iridiumsbd)
+add_subdirectory(iridiumsbd)

src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp

@@ -235,7 +235,7 @@ int IridiumSBD::print_status()
 	PX4_INFO("RX session pending:           %d", _rx_session_pending);
 	PX4_INFO("RX read pending:              %d", _rx_read_pending);
 	PX4_INFO("Time since last signal check: %" PRId64, hrt_absolute_time() - _last_signal_check);
-	PX4_INFO("Last heartbeat:               %" PRId64, _last_heartbeat);
+	PX4_INFO("Last heartbeat:               %" PRId64, _last_at_ok_timestamp);
 	return 0;
 }
 
@@ -333,6 +333,11 @@ void IridiumSBD::standby_loop(void)
 		}
 	}
 
+	if (!is_modem_responsive()) {
+		VERBOSE_INFO("MODEM IS NOT RENSPONSIVE");
+		return;
+	}
+
 	// check for incoming SBDRING, handled inside read_at_command()
 	read_at_command();
 
@@ -477,7 +482,6 @@ void IridiumSBD::sbdsession_loop(void)
 		_ring_pending = false;
 		_tx_session_pending = false;
 		_last_read_time = hrt_absolute_time();
-		_last_heartbeat = _last_read_time;
 		++_successful_sbd_sessions;
 
 		if (mt_queued > 0) {
@@ -498,8 +502,6 @@ void IridiumSBD::sbdsession_loop(void)
 
 	case 1:
 		VERBOSE_INFO("SBD SESSION: MO SUCCESS, MT FAIL");
-		_last_heartbeat = hrt_absolute_time();
-
 		// after a successful session reset the tx buffer
 		_tx_buf_write_idx = 0;
 		++_successful_sbd_sessions;
@@ -552,11 +554,6 @@ void IridiumSBD::start_csq(void)
 
 	_last_signal_check = hrt_absolute_time();
 
-	if (!is_modem_ready()) {
-		VERBOSE_INFO("UPDATE SIGNAL QUALITY: MODEM NOT READY!");
-		return;
-	}
-
 	write_at("AT+CSQ");
 	_new_state = SATCOM_STATE_CSQ;
 }
@@ -571,11 +568,6 @@ void IridiumSBD::start_sbd_session(void)
 		VERBOSE_INFO("STARTING SBD SESSION");
 	}
 
-	if (!is_modem_ready()) {
-		VERBOSE_INFO("SBD SESSION: MODEM NOT READY!");
-		return;
-	}
-
 	if (_ring_pending) {
 		write_at("AT+SBDIXA");
 
@@ -610,8 +602,8 @@ void IridiumSBD::start_test(void)
 		printf("\n");
 	}
 
-	if (!is_modem_ready()) {
-		PX4_WARN("MODEM NOT READY!");
+	if (!is_modem_responsive()) {
+		PX4_WARN("MODEM NOT RENSPONSIVE!");
 		return;
 	}
 
@@ -718,11 +710,6 @@ ssize_t IridiumSBD::read(struct file *filp, char *buffer, size_t buflen)
 
 void IridiumSBD::write_tx_buf()
 {
-	if (!is_modem_ready()) {
-		VERBOSE_INFO("WRITE SBD: MODEM NOT READY!");
-		return;
-	}
-
 	pthread_mutex_lock(&_tx_buf_mutex);
 
 	char command[13];
@@ -779,11 +766,6 @@ void IridiumSBD::write_tx_buf()
 
 void IridiumSBD::read_rx_buf(void)
 {
-	if (!is_modem_ready()) {
-		VERBOSE_INFO("READ SBD: MODEM NOT READY!");
-		return;
-	}
-
 	pthread_mutex_lock(&_rx_buf_mutex);
 
 
@@ -949,11 +931,12 @@ satcom_uart_status IridiumSBD::open_uart(char *uart_name)
 	return SATCOM_UART_OK;
 }
 
-bool IridiumSBD::is_modem_ready(void)
+bool IridiumSBD::is_modem_responsive(void)
 {
 	write_at("AT");
 
 	if (read_at_command() == SATCOM_RESULT_OK) {
+		_last_at_ok_timestamp = hrt_absolute_time();
 		return true;
 
 	} else {
@@ -980,9 +963,9 @@ void IridiumSBD::publish_iridium_status()
 {
 	bool need_to_publish = false;
 
-	if (_status.last_heartbeat != _last_heartbeat) {
+	if (_status.last_at_ok_timestamp != _last_at_ok_timestamp) {
 		need_to_publish = true;
-		_status.last_heartbeat = _last_heartbeat;
+		_status.last_at_ok_timestamp = _last_at_ok_timestamp;
 	}
 
 	if (_status.tx_buf_write_index != _tx_buf_write_idx) {

src/drivers/telemetry/iridiumsbd/IridiumSBD.h

@@ -245,7 +245,7 @@ class IridiumSBD : public cdev::CDev, public ModuleBase<IridiumSBD>
 	/*
 	 * Checks if the modem responds to the "AT" command
 	 */
-	bool is_modem_ready(void);
+	bool is_modem_responsive(void);
 
 	/*
 	 * Get the poll state
@@ -321,7 +321,7 @@ class IridiumSBD : public cdev::CDev, public ModuleBase<IridiumSBD>
 
 	hrt_abstime _last_write_time = 0;
 	hrt_abstime _last_read_time = 0;
-	hrt_abstime _last_heartbeat = 0;
+	hrt_abstime _last_at_ok_timestamp = 0;
 	hrt_abstime _session_start_time = 0;
 
 	satcom_state _state = SATCOM_STATE_STANDBY;

src/modules/commander/Commander.cpp

@@ -1109,7 +1109,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: {
 			// if no high latency telemetry exists send a failed acknowledge
-			if (_high_latency_datalink_heartbeat > _boot_timestamp) {
+			if (_high_latency_datalink_timestamp < _boot_timestamp) {
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
 				mavlink_log_critical(&_mavlink_log_pub, "Control high latency failed! Telemetry unavailable\t");
 				events::send(events::ID("commander_ctrl_high_latency_failed"), {events::Log::Critical, events::LogInternal::Info},
@@ -2672,6 +2672,28 @@ void Commander::enable_hil()
 
 void Commander::dataLinkCheck()
 {
+	// high latency data link
+	iridiumsbd_status_s iridium_status;
+
+	if (_iridiumsbd_status_sub.update(&iridium_status)) {
+		_high_latency_datalink_timestamp = iridium_status.last_at_ok_timestamp;
+
+		if (_vehicle_status.high_latency_data_link_lost &&
+		    (_high_latency_datalink_timestamp > _high_latency_datalink_lost) &&
+		    (_high_latency_datalink_regained == 0)
+		   ) {
+			_high_latency_datalink_regained = _high_latency_datalink_timestamp;
+		}
+
+		if (_vehicle_status.high_latency_data_link_lost &&
+		    (_high_latency_datalink_regained != 0) &&
+		    (hrt_elapsed_time(&_high_latency_datalink_regained) > (_param_com_hldl_reg_t.get() * 1_s))
+		   ) {
+			_vehicle_status.high_latency_data_link_lost = false;
+			_status_changed = true;
+		}
+	}
+
 	for (auto &telemetry_status :  _telemetry_status_subs) {
 		telemetry_status_s telemetry;
 
@@ -2685,16 +2707,18 @@ void Commander::dataLinkCheck()
 				break;
 
 			case telemetry_status_s::LINK_TYPE_IRIDIUM: {
-					iridiumsbd_status_s iridium_status;
 
-					if (_iridiumsbd_status_sub.update(&iridium_status)) {
-						_high_latency_datalink_heartbeat = iridium_status.last_heartbeat;
+					if ((_high_latency_datalink_timestamp > 0) &&
+					    (hrt_elapsed_time(&_high_latency_datalink_timestamp) > (_param_com_hldl_loss_t.get() * 1_s))) {
 
-						if (_vehicle_status.high_latency_data_link_lost) {
-							if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) {
-								_vehicle_status.high_latency_data_link_lost = false;
-								_status_changed = true;
-							}
+						_high_latency_datalink_lost = _high_latency_datalink_timestamp;
+						_high_latency_datalink_regained = 0;
+
+						if (!_vehicle_status.high_latency_data_link_lost) {
+							_vehicle_status.high_latency_data_link_lost = true;
+							mavlink_log_critical(&_mavlink_log_pub, "High latency data link lost\t");
+							events::send(events::ID("commander_high_latency_lost"), events::Log::Critical, "High latency data link lost");
+							_status_changed = true;
 						}
 					}
 
@@ -2836,19 +2860,6 @@ void Commander::dataLinkCheck()
 			_vehicle_status.avoidance_system_valid = false;
 		}
 	}
-
-	// high latency data link loss failsafe
-	if (_high_latency_datalink_heartbeat > 0
-	    && hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_param_com_hldl_loss_t.get() * 1_s)) {
-		_high_latency_datalink_lost = hrt_absolute_time();
-
-		if (!_vehicle_status.high_latency_data_link_lost) {
-			_vehicle_status.high_latency_data_link_lost = true;
-			mavlink_log_critical(&_mavlink_log_pub, "High latency data link lost\t");
-			events::send(events::ID("commander_high_latency_lost"), events::Log::Critical, "High latency data link lost");
-			_status_changed = true;
-		}
-	}
 }
 
 void Commander::battery_status_check()

src/modules/commander/Commander.hpp

@@ -246,8 +246,9 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 
 	hrt_abstime _last_print_mode_reject_time{0};	///< To remember when last notification was sent
 
-	hrt_abstime _high_latency_datalink_heartbeat{0};
+	hrt_abstime _high_latency_datalink_timestamp{0};
 	hrt_abstime _high_latency_datalink_lost{0};
+	hrt_abstime _high_latency_datalink_regained{0};
 
 	hrt_abstime _boot_timestamp{0};
 	hrt_abstime _last_disarmed_timestamp{0};

src/modules/mavlink/mavlink_events.cpp

@@ -208,17 +208,18 @@ void SendProtocol::handle_request_event(const mavlink_message_t &msg) const
 
 void SendProtocol::send_event(const Event &event) const
 {
-	mavlink_event_t event_msg{};
-	event_msg.event_time_boot_ms = event.timestamp_ms;
-	event_msg.destination_component = MAV_COMP_ID_ALL;
-	event_msg.destination_system = 0;
-	event_msg.id = event.id;
-	event_msg.sequence = event.sequence;
-	event_msg.log_levels = event.log_levels;
-	static_assert(sizeof(event_msg.arguments) >= sizeof(event.arguments), "MAVLink message arguments buffer too small");
-	memcpy(&event_msg.arguments, event.arguments, sizeof(event.arguments));
-	mavlink_msg_event_send_struct(_mavlink.get_channel(), &event_msg);
-
+	if (_mavlink.get_mode() != Mavlink::MAVLINK_MODE_IRIDIUM) {
+		mavlink_event_t event_msg{};
+		event_msg.event_time_boot_ms = event.timestamp_ms;
+		event_msg.destination_component = MAV_COMP_ID_ALL;
+		event_msg.destination_system = 0;
+		event_msg.id = event.id;
+		event_msg.sequence = event.sequence;
+		event_msg.log_levels = event.log_levels;
+		static_assert(sizeof(event_msg.arguments) >= sizeof(event.arguments), "MAVLink message arguments buffer too small");
+		memcpy(&event_msg.arguments, event.arguments, sizeof(event.arguments));
+		mavlink_msg_event_send_struct(_mavlink.get_channel(), &event_msg);
+	}
 }
 
 void SendProtocol::on_gcs_connected()
@@ -228,8 +229,9 @@ void SendProtocol::on_gcs_connected()
 
 void SendProtocol::send_current_sequence(const hrt_abstime &now, bool force_reset)
 {
-	// only send if enough tx buffer space available
-	if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
+	// only send if enough tx buffer space available or not MAVLINK_MODE_IRIDIUM
+	if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES ||
+	    _mavlink.get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
 		return;
 	}