Improve high latency switching and acknowledge

- Move publishing the telemetry status from the IridiumSBD driver to the mavlink instance
- In the commander use the iridiumsbd_status message for heartbeat in case of a high latency link
- Move positive acknowledge to the mavlink instance
- Add a failed acknowledge in the commander if no high latency link exists

src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp

@@ -401,8 +401,6 @@ void IridiumSBD::csq_loop(void)
 	VERBOSE_INFO("SIGNAL QUALITY: %d", _signal_quality);
 
 	_new_state = SATCOM_STATE_STANDBY;
-
-	publish_telemetry_status();
 }
 
 void IridiumSBD::sbdsession_loop(void)
@@ -417,7 +415,6 @@ void IridiumSBD::sbdsession_loop(void)
 			PX4_WARN("SBD SESSION: TIMEOUT!");
 			++_failed_sbd_sessions;
 			_new_state = SATCOM_STATE_STANDBY;
-			publish_telemetry_status();
 			pthread_mutex_unlock(&_tx_buf_mutex);
 		}
 
@@ -429,7 +426,6 @@ void IridiumSBD::sbdsession_loop(void)
 
 		++_failed_sbd_sessions;
 		_new_state = SATCOM_STATE_STANDBY;
-		publish_telemetry_status();
 		pthread_mutex_unlock(&_tx_buf_mutex);
 		return;
 	}
@@ -440,7 +436,6 @@ void IridiumSBD::sbdsession_loop(void)
 
 		_new_state = SATCOM_STATE_STANDBY;
 		++_failed_sbd_sessions;
-		publish_telemetry_status();
 		pthread_mutex_unlock(&_tx_buf_mutex);
 		return;
 	}
@@ -510,7 +505,6 @@ void IridiumSBD::sbdsession_loop(void)
 		VERBOSE_INFO("SBD SESSION: FAILED (%d)", mo_status);
 	}
 
-	publish_telemetry_status();
 	_new_state = SATCOM_STATE_STANDBY;
 	pthread_mutex_unlock(&_tx_buf_mutex);
 }
@@ -661,7 +655,6 @@ ssize_t IridiumSBD::write(struct file *filp, const char *buffer, size_t buflen)
 	if (SATCOM_TX_BUF_LEN - _tx_buf_write_idx - _packet_length < 0) {
 		_tx_buf_write_idx = 0;
 		++_num_tx_buf_reset;
-		publish_telemetry_status();
 	}
 
 	// keep track of the remaining packet length and if the full message is written
@@ -679,8 +672,6 @@ ssize_t IridiumSBD::write(struct file *filp, const char *buffer, size_t buflen)
 	_last_write_time = hrt_absolute_time();
 	_tx_buf_write_pending = true;
 
-	publish_telemetry_status();
-
 	pthread_mutex_unlock(&_tx_buf_mutex);
 
 	return buflen;
@@ -958,28 +949,6 @@ pollevent_t IridiumSBD::poll_state(struct file *filp)
 	return pollstate;
 }
 
-void IridiumSBD::publish_telemetry_status()
-{
-	// publish telemetry status for logger
-	struct telemetry_status_s tstatus = {};
-
-	tstatus.timestamp = hrt_absolute_time();
-	tstatus.telem_time = tstatus.timestamp;
-	tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM;
-	tstatus.rssi = _signal_quality;
-	tstatus.txbuf = ceil(100.0f * (float)_tx_buf_write_idx / SATCOM_TX_BUF_LEN);
-	tstatus.heartbeat_time = _last_heartbeat;
-	tstatus.rxerrors = _failed_sbd_sessions;
-
-	if (_telemetry_status_pub == nullptr) {
-		int multi_instance;
-		_telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_LOW);
-
-	} else {
-		orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
-	}
-}
-
 void IridiumSBD::publish_iridium_status()
 {
 	bool need_to_publish = false;

src/drivers/telemetry/iridiumsbd/IridiumSBD.h

@@ -40,7 +40,6 @@
 #include <drivers/drv_hrt.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/iridiumsbd_status.h>
 
 typedef enum {
@@ -255,11 +254,6 @@ private:
 	 */
 	pollevent_t poll_state(struct file *filp);
 
-	/*
-	 * Publish the up to date telemetry status
-	 */
-	void publish_telemetry_status(void);
-
 	void publish_iridium_status(void);
 
 	/**
@@ -306,7 +300,6 @@ private:
 	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;

src/modules/commander/Commander.hpp

@@ -45,6 +45,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/home_position.h>
+#include <uORB/topics/iridiumsbd_status.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
@@ -168,13 +169,11 @@ private:
 		bool high_latency = false;
 	} _telemetry[ORB_MULTI_MAX_INSTANCES];
 
-	// publisher
-	orb_advert_t _vehicle_cmd_pub = nullptr;
-
 	// Subscriptions
 	Subscription<mission_result_s>			_mission_result_sub;
 	Subscription<vehicle_global_position_s>		_global_position_sub;
 	Subscription<vehicle_local_position_s>		_local_position_sub;
+	Subscription<iridiumsbd_status_s> 		_iridiumsbd_status_sub;
 };
 
 #endif /* COMMANDER_HPP_ */

src/modules/commander/commander.cpp

@@ -587,7 +587,8 @@ Commander::Commander() :
 	ModuleParams(nullptr),
 	_mission_result_sub(ORB_ID(mission_result)),
 	_global_position_sub(ORB_ID(vehicle_global_position)),
-	_local_position_sub(ORB_ID(vehicle_local_position))
+	_local_position_sub(ORB_ID(vehicle_local_position)),
+	_iridiumsbd_status_sub(ORB_ID(iridiumsbd_status))
 {
 }
 
@@ -1020,10 +1021,17 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
 	}
 	break;
 	case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: {
-			// only send the acknowledge from the commander, the command actually is handled by each mavlink instance
-			// only send the acknowledge if the command is received from an external source
-			if (cmd.from_external) {
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+			bool hl_exists = false;
+			for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+				if (_telemetry[i].high_latency) {
+					hl_exists = true;
+				}
+			}
+
+			// if no high latency telemetry exists send a failed acknowledge
+			if (!hl_exists) {
+				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED;
+				mavlink_log_critical(&mavlink_log_pub, "Control high latency failed, no hl telemetry available");
 			}
 		}
 		break;
@@ -4100,6 +4108,17 @@ void Commander::poll_telemetry_status()
 				_telemetry[i].last_heartbeat = telemetry.heartbeat_time;
 			}
 		}
+
+		// for iridium telemetry use the iridiumsbd_status to update the heartbeat
+		if (_telemetry[i].high_latency) {
+			if (_iridiumsbd_status_sub.update()) {
+				const hrt_abstime isbd_timestamp = _iridiumsbd_status_sub.get().last_heartbeat;
+
+				if (isbd_timestamp > _telemetry[i].last_heartbeat) {
+					_telemetry[i].last_heartbeat = isbd_timestamp;
+				}
+			}
+		}
 	}
 }
 
@@ -4193,21 +4212,6 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32
 			status.high_latency_data_link_active = false;
 			*status_changed = true;
 			mavlink_log_critical(&mavlink_log_pub, "LOW LATENCY DATA LINKS REGAINED, DEACTIVATING HIGH LATENCY LINK");
-
-			vehicle_command_s vehicle_cmd;
-			vehicle_cmd.timestamp = hrt_absolute_time();
-			vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY;
-			vehicle_cmd.param1 = 0.0f;
-			vehicle_cmd.from_external = false;
-			vehicle_cmd.target_system = status.system_id;
-			vehicle_cmd.target_component = 0;
-
-			if (_vehicle_cmd_pub != nullptr) {
-				orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vehicle_cmd);
-
-			} else {
-				_vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd);
-			}
 		}
 
 	} else {
@@ -4216,14 +4220,6 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32
 			status.high_latency_data_link_active = true;
 			*status_changed = true;
 
-			vehicle_command_s vehicle_cmd;
-			vehicle_cmd.timestamp = hrt_absolute_time();
-			vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY;
-			vehicle_cmd.param1 = 1.0f;
-			vehicle_cmd.from_external = false;
-			vehicle_cmd.target_system = status.system_id;
-			vehicle_cmd.target_component = 0;
-
 			// set heartbeat to current time for high latency so that the first message can be transmitted
 			for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
 				if (_telemetry[i].high_latency) {
@@ -4231,13 +4227,6 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32
 				}
 			}
 
-			if (_vehicle_cmd_pub != nullptr) {
-				orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vehicle_cmd);
-
-			} else {
-				_vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd);
-			}
-
 			if (!status.data_link_lost) {
 				mavlink_log_critical(&mavlink_log_pub, "ALL LOW LATENCY DATA LINKS LOST, ACTIVATING HIGH LATENCY LINK");
 

src/modules/mavlink/mavlink_main.cpp

@@ -1972,6 +1972,9 @@ Mavlink::task_main(int argc, char *argv[])
 	ack_sub->subscribe_from_beginning(true);
 	cmd_sub->subscribe_from_beginning(true);
 
+	/* command ack */
+	orb_advert_t command_ack_pub = nullptr;
+
 	MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
 
 	struct vehicle_status_s status;
@@ -2202,6 +2205,20 @@ Mavlink::task_main(int argc, char *argv[])
 			set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);
 
 			set_manual_input_mode_generation(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED);
+
+			if (_mode == MAVLINK_MODE_IRIDIUM) {
+				if (_transmitting_enabled &&
+				    !status.high_latency_data_link_active &&
+				    !_transmitting_enabled_commanded &&
+				    (_last_write_success_time > 0u)) { // a first message is written
+					_transmitting_enabled = false;
+					mavlink_and_console_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s", _device_name);
+
+				} else if (!_transmitting_enabled && status.high_latency_data_link_active) {
+					_transmitting_enabled = true;
+					mavlink_and_console_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s", _device_name);
+				}
+			}
 		}
 
 		struct vehicle_command_s vehicle_cmd;
@@ -2211,7 +2228,8 @@ Mavlink::task_main(int argc, char *argv[])
 			    (_mode == MAVLINK_MODE_IRIDIUM)) {
 				if (vehicle_cmd.param1 > 0.5f) {
 					if (!_transmitting_enabled) {
-						PX4_INFO("Enable transmitting with IRIDIUM mavlink on device %s by command", _device_name);
+						mavlink_and_console_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command",
+									     _device_name);
 					}
 
 					_transmitting_enabled = true;
@@ -2219,12 +2237,33 @@ Mavlink::task_main(int argc, char *argv[])
 
 				} else {
 					if (_transmitting_enabled) {
-						PX4_INFO("Disable transmitting with IRIDIUM mavlink on device %s by command", _device_name);
+						mavlink_and_console_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command",
+									     _device_name);
 					}
 
 					_transmitting_enabled = false;
 					_transmitting_enabled_commanded = false;
 				}
+
+				// send positive command ack
+				struct vehicle_command_ack_s command_ack = {
+					.timestamp = vehicle_cmd.timestamp,
+					.result_param2 = 0,
+					.command = vehicle_cmd.command,
+					.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED,
+					.from_external = !vehicle_cmd.from_external,
+					.result_param1 = 0,
+					.target_system = vehicle_cmd.source_system,
+					.target_component = vehicle_cmd.source_component
+				};
+
+				if (command_ack_pub != nullptr) {
+					orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
+
+				} else {
+					command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
+									      vehicle_command_ack_s::ORB_QUEUE_LENGTH);
+				}
 			}
 		}
 
@@ -2335,13 +2374,6 @@ Mavlink::task_main(int argc, char *argv[])
 			stream->update(t);
 		}
 
-		if (_mode == MAVLINK_MODE_IRIDIUM) {
-			if ((_last_write_success_time > 0u) && !_transmitting_enabled_commanded && _transmitting_enabled) {
-				_transmitting_enabled = false;
-				PX4_INFO("Disable Iridium Mavlink after first packet is sent");
-			}
-		}
-
 		/* pass messages from other UARTs */
 		if (_forwarding_on) {
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -2408,6 +2408,22 @@ MavlinkReceiver::receive_thread(void *arg)
 	// poll timeout in ms. Also defines the max update frequency of the mission & param manager, etc.
 	const int timeout = 10;
 
+	// publish the telemetry status once for the iridium telemetry
+	if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
+		struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
+
+		tstatus.timestamp = hrt_absolute_time();
+		tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM;
+
+		if (_telemetry_status_pub == nullptr) {
+			int multi_instance;
+			_telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH);
+
+		} else {
+			orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
+		}
+	}
+
 #ifdef __PX4_POSIX
 	/* 1500 is the Wifi MTU, so we make sure to fit a full packet */
 	uint8_t buf[1600 * 5];