mavlink: publish telemetry_status per instance with all HEARTBEATS from the same system

- one telemetry_status publication per mavlink instance
 - each telemetry_status has an array of HEARTBEATS

msg/CMakeLists.txt

@@ -125,6 +125,7 @@ set(msg_files
 	system_power.msg
 	task_stack_info.msg
 	tecs_status.msg
+	telemetry_heartbeat.msg
 	telemetry_status.msg
 	test_motor.msg
 	timesync.msg

msg/telemetry_heartbeat.msg

@@ -0,0 +1,51 @@
+uint64 timestamp          # time since system start (microseconds)
+
+
+# COMPONENT (fill in as needed)
+uint8 COMP_ID_ALL                      = 0
+uint8 COMP_ID_AUTOPILOT1               = 1
+uint8 COMP_ID_TELEMETRY_RADIO          = 68
+uint8 COMP_ID_CAMERA                   = 100
+uint8 COMP_ID_GIMBAL                   = 154
+uint8 COMP_ID_LOG                      = 155
+uint8 COMP_ID_ADSB                     = 156
+uint8 COMP_ID_OSD                      = 157
+uint8 COMP_ID_PERIPHERAL               = 158
+uint8 COMP_ID_FLARM                    = 160
+uint8 COMP_ID_MISSIONPLANNER           = 190
+uint8 COMP_ID_OBSTACLE_AVOIDANCE       = 196
+uint8 COMP_ID_VISUAL_INERTIAL_ODOMETRY = 197
+uint8 COMP_ID_PAIRING_MANAGER          = 198
+uint8 COMP_ID_UDP_BRIDGE               = 240
+uint8 COMP_ID_UART_BRIDGE              = 241
+uint8 COMP_ID_TUNNEL_NODE              = 242
+
+uint8 system_id           # system id of the remote system (Mavlink header sys_id)
+uint8 component_id        # component id of the remote system (Mavlink header comp_id)
+
+
+# TYPE (fill in as needed)
+uint8 TYPE_GENERIC                     = 0
+uint8 TYPE_ANTENNA_TRACKER             = 5
+uint8 TYPE_GCS                         = 6
+uint8 TYPE_ONBOARD_CONTROLLER          = 18
+uint8 TYPE_GIMBAL                      = 26
+uint8 TYPE_ADSB                        = 27
+uint8 TYPE_CAMERA                      = 30
+uint8 TYPE_CHARGING_STATION            = 31
+
+uint8 type
+
+
+# STATE
+uint8 STATE_UNINIT                     = 0
+uint8 STATE_BOOT                       = 1
+uint8 STATE_CALIBRATING                = 2
+uint8 STATE_STANDBY                    = 3
+uint8 STATE_ACTIVE                     = 4
+uint8 STATE_CRITICAL                   = 5
+uint8 STATE_EMERGENCY                  = 6
+uint8 STATE_POWEROFF                   = 7
+uint8 STATE_FLIGHT_TERMINATION         = 8
+
+uint8 state

msg/telemetry_status.msg

@@ -4,36 +4,9 @@ uint8 LINK_TYPE_WIRE = 2
 uint8 LINK_TYPE_USB = 3
 uint8 LINK_TYPE_IRIDIUM	= 4
 
-# MAV_COMPONENT (fill in as needed)
-uint8 COMPONENT_ID_ALL		= 0
-uint8 COMPONENT_ID_AUTOPILOT1	= 1
-uint8 COMPONENT_ID_CAMERA	= 100
-uint8 COMPONENT_ID_OBSTACLE_AVOIDANCE = 196
-
-# MAV_TYPE (fill in as needed)
-uint8 MAV_TYPE_GENERIC = 0
-uint8 MAV_TYPE_GCS = 6
-uint8 MAV_TYPE_ONBOARD_CONTROLLER = 18
-
-# MAV_STATE
-uint8 MAV_STATE_UNINIT = 0
-uint8 MAV_STATE_BOOT = 1
-uint8 MAV_STATE_CALIBRATING = 2
-uint8 MAV_STATE_STANDBY = 3
-uint8 MAV_STATE_ACTIVE = 4
-uint8 MAV_STATE_CRITICAL = 5
-uint8 MAV_STATE_EMERGENCY = 6
-uint8 MAV_STATE_POWEROFF = 7
-uint8 MAV_STATE_FLIGHT_TERMINATION = 8
-
 uint64 timestamp			# time since system start (microseconds)
 
-uint64 heartbeat_time			# Time of last received heartbeat from remote system
+telemetry_heartbeat[4] heartbeats
-
-uint8 remote_system_id			# system id of the remote system (Mavlink header sys_id)
-uint8 remote_component_id		# component id of the remote system (Mavlink header comp_id)
-uint8 remote_type			# remote system status flag (Mavlink MAV_TYPE)
-uint8 remote_system_status		# remote system status flag (Mavlink MAV_STATE)
 
 uint8 type				#  type of the radio hardware (LINK_TYPE_*)
 
@@ -54,5 +27,3 @@ float32 rate_rx
 
 float32 rate_tx
 float32 rate_txerr
-
-uint8 ORB_QUEUE_LENGTH = 3

msg/tools/uorb_rtps_message_ids.yaml

@@ -295,6 +295,8 @@ rtps:
     id: 131
   - msg: yaw_estimator_status
     id: 132
+  - msg: telemetry_heartbeat
+    id: 133
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

src/modules/commander/Commander.cpp

@@ -3716,11 +3716,10 @@ void Commander::mission_init()
 
 void Commander::data_link_check()
 {
-	if (_telemetry_status_sub.updated()) {
+	for (auto &telemetry_status :  _telemetry_status_sub) {
-
 		telemetry_status_s telemetry;
 
-		if (_telemetry_status_sub.copy(&telemetry)) {
+		if (telemetry_status.update(&telemetry)) {
 
 			// handle different radio types
 			switch (telemetry.type) {
@@ -3730,7 +3729,6 @@ void Commander::data_link_check()
 				break;
 
 			case telemetry_status_s::LINK_TYPE_IRIDIUM: {
-
 					iridiumsbd_status_s iridium_status;
 
 					if (_iridiumsbd_status_sub.update(&iridium_status)) {
@@ -3748,65 +3746,65 @@ void Commander::data_link_check()
 				}
 			}
 
-			// handle different remote types
+			for (const auto &hb : telemetry.heartbeats) {
-			switch (telemetry.remote_type) {
+				// handle different remote types
-			case telemetry_status_s::MAV_TYPE_GCS:
+				switch (hb.type) {
+				case telemetry_heartbeat_s::TYPE_GCS:
 
-				// Initial connection or recovery from data link lost
+					// Initial connection or recovery from data link lost
-				if (status.data_link_lost) {
+					if (status.data_link_lost) {
-					if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) {
+						if (hb.timestamp > _datalink_last_heartbeat_gcs) {
-						status.data_link_lost = false;
+							status.data_link_lost = false;
-						_status_changed = true;
+							_status_changed = true;
 
-						if (!armed.armed && !status_flags.condition_calibration_enabled) {
+							if (!armed.armed && !status_flags.condition_calibration_enabled) {
-							// make sure to report preflight check failures to a connecting GCS
+								// make sure to report preflight check failures to a connecting GCS
-							PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags,
+								PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags,
-										       _arm_requirements.global_position, true, true, hrt_elapsed_time(&_boot_timestamp));
+											       _arm_requirements.global_position, true, true, hrt_elapsed_time(&_boot_timestamp));
-						}
+							}
 
-						if (_datalink_last_heartbeat_gcs != 0) {
+							if (_datalink_last_heartbeat_gcs != 0) {
-							mavlink_log_info(&mavlink_log_pub, "Data link regained");
+								mavlink_log_info(&mavlink_log_pub, "Data link regained");
+							}
 						}
 					}
-				}
 
-				// Only keep the very last heartbeat timestamp, so we don't get confused
+					// Only keep the very last heartbeat timestamp, so we don't get confused
-				// by multiple mavlink instances publishing different timestamps.
+					// by multiple mavlink instances publishing different timestamps.
-				if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) {
+					if (hb.timestamp > _datalink_last_heartbeat_gcs) {
-					_datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
+						_datalink_last_heartbeat_gcs = hb.timestamp;
-				}
-
-				break;
-
-			case telemetry_status_s::MAV_TYPE_ONBOARD_CONTROLLER:
-
-				if (_onboard_controller_lost) {
-					if (telemetry.heartbeat_time > _datalink_last_heartbeat_onboard_controller) {
-						mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
-						_onboard_controller_lost = false;
-						_status_changed = true;
 					}
 
-				}
+					break;
 
-				_datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;
+				case telemetry_heartbeat_s::TYPE_ONBOARD_CONTROLLER:
-
+					if (_onboard_controller_lost) {
-				if (telemetry.remote_component_id == telemetry_status_s::COMPONENT_ID_OBSTACLE_AVOIDANCE) {
+						if (hb.timestamp > _datalink_last_heartbeat_onboard_controller) {
-					if (telemetry.heartbeat_time != _datalink_last_heartbeat_avoidance_system) {
+							mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
-						_avoidance_system_status_change = _datalink_last_status_avoidance_system != telemetry.remote_system_status;
+							_onboard_controller_lost = false;
+							_status_changed = true;
+						}
 					}
 
-					_datalink_last_heartbeat_avoidance_system = telemetry.heartbeat_time;
+					_datalink_last_heartbeat_onboard_controller = hb.timestamp;
-					_datalink_last_status_avoidance_system = telemetry.remote_system_status;
 
-					if (_avoidance_system_lost) {
+					if (hb.component_id == telemetry_heartbeat_s::COMP_ID_OBSTACLE_AVOIDANCE) {
-						_status_changed = true;
+						if (hb.timestamp > _datalink_last_heartbeat_avoidance_system) {
-						_avoidance_system_lost = false;
+							_avoidance_system_status_change = (_datalink_last_status_avoidance_system != hb.state);
-						status_flags.avoidance_system_valid = true;
+						}
+
+						_datalink_last_heartbeat_avoidance_system = hb.timestamp;
+						_datalink_last_status_avoidance_system = hb.state;
+
+						if (_avoidance_system_lost) {
+							_status_changed = true;
+							_avoidance_system_lost = false;
+							status_flags.avoidance_system_valid = true;
+						}
 					}
-				}
 
-				break;
+					break;
+				}
 			}
 		}
 	}
@@ -3814,7 +3812,7 @@ void Commander::data_link_check()
 
 	// GCS data link loss failsafe
 	if (!status.data_link_lost) {
-		if (_datalink_last_heartbeat_gcs != 0
+		if ((_datalink_last_heartbeat_gcs != 0)
 		    && hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_param_com_dl_loss_t.get() * 1_s)) {
 
 			status.data_link_lost = true;
@@ -3842,26 +3840,27 @@ void Commander::data_link_check()
 		//if heartbeats stop
 		if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0)
 		    && (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) {
+
 			_avoidance_system_lost = true;
 			status_flags.avoidance_system_valid = false;
 		}
 
 		//if status changed
 		if (_avoidance_system_status_change) {
-			if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_BOOT) {
+			if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_BOOT) {
 				status_flags.avoidance_system_valid = false;
 			}
 
-			if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_ACTIVE) {
+			if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_ACTIVE) {
 				status_flags.avoidance_system_valid = true;
 			}
 
-			if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_CRITICAL) {
+			if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_CRITICAL) {
 				status_flags.avoidance_system_valid = false;
 				_status_changed = true;
 			}
 
-			if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION) {
+			if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_FLIGHT_TERMINATION) {
 				status_flags.avoidance_system_valid = false;
 				_status_changed = true;
 			}

src/modules/commander/Commander.hpp

@@ -320,7 +320,7 @@ private:
 	bool		_onboard_controller_lost{false};
 	bool		_avoidance_system_lost{false};
 	bool		_avoidance_system_status_change{false};
-	uint8_t		_datalink_last_status_avoidance_system{telemetry_status_s::MAV_STATE_UNINIT};
+	uint8_t		_datalink_last_status_avoidance_system{telemetry_heartbeat_s::STATE_UNINIT};
 
 	hrt_abstime	_high_latency_datalink_heartbeat{0};
 	hrt_abstime	_high_latency_datalink_lost{0};
@@ -406,7 +406,7 @@ private:
 	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription					_subsys_sub{ORB_ID(subsystem_info)};
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
-	uORB::Subscription					_telemetry_status_sub{ORB_ID(telemetry_status)};
+	uORB::Subscription					_telemetry_status_sub[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(telemetry_status), 0}, {ORB_ID(telemetry_status), 1}, {ORB_ID(telemetry_status), 2}, {ORB_ID(telemetry_status), 3}};
 	uORB::Subscription					_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
 

src/modules/mavlink/mavlink_main.cpp

@@ -355,6 +355,18 @@ Mavlink::get_instance_for_network_port(unsigned long port)
 }
 #endif // MAVLINK_UDP
 
+bool
+Mavlink::is_connected()
+{
+	for (auto &hb : _tstatus.heartbeats) {
+		if ((hb.type == telemetry_heartbeat_s::TYPE_GCS) && (hrt_elapsed_time(&hb.timestamp) < 3_s)) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
 int
 Mavlink::destroy_all_instances()
 {
@@ -826,7 +838,7 @@ void Mavlink::send_finish()
 # endif // CONFIG_NET
 
 		if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() &&
-		    (!get_client_source_initialized() || (hrt_elapsed_time(&_tstatus.heartbeat_time) > 3_s))) {
+		    (!get_client_source_initialized() || !is_connected())) {
 
 			if (!_broadcast_address_found) {
 				find_broadcast_address();
@@ -2103,6 +2115,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 	/* initialize send mutex */
 	pthread_mutex_init(&_send_mutex, nullptr);
+	pthread_mutex_init(&_telemetry_status_mutex, nullptr);
 
 	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
 	if (_forwarding_on) {
@@ -2469,7 +2482,7 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 
 		// publish status at 1 Hz, or sooner if HEARTBEAT has updated
-		if ((hrt_elapsed_time(&_tstatus.timestamp) >= 1_s) || (_tstatus.timestamp < _tstatus.heartbeat_time)) {
+		if ((hrt_elapsed_time(&_tstatus.timestamp) >= 1_s) || _tstatus_updated) {
 			publish_telemetry_status();
 		}
 
@@ -2505,6 +2518,9 @@ Mavlink::task_main(int argc, char *argv[])
 		_mavlink_ulog = nullptr;
 	}
 
+	pthread_mutex_destroy(&_send_mutex);
+	pthread_mutex_destroy(&_telemetry_status_mutex);
+
 	PX4_INFO("exiting channel %i", (int)_channel);
 
 	return OK;
@@ -2596,9 +2612,12 @@ void Mavlink::publish_telemetry_status()
 
 	_tstatus.streams = _streams.size();
 
+	// telemetry_status is also updated from the receiver thread, but never the same fields
+	lock_telemetry_status();
 	_tstatus.timestamp = hrt_absolute_time();
-
 	_telem_status_pub.publish(_tstatus);
+	_tstatus_updated = false;
+	unlock_telemetry_status();
 }
 
 void Mavlink::configure_sik_radio()
@@ -2737,8 +2756,10 @@ Mavlink::start(int argc, char *argv[])
 void
 Mavlink::display_status()
 {
-	if (_tstatus.heartbeat_time > 0) {
+	for (const auto &hb : _tstatus.heartbeats) {
-		printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&_tstatus.heartbeat_time));
+		if ((hb.timestamp > 0) && (hb.type == telemetry_heartbeat_s::TYPE_GCS)) {
+			printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&hb.timestamp));
+		}
 	}
 
 	printf("\tmavlink chan: #%u\n", _channel);

src/modules/mavlink/mavlink_main.h

@@ -71,7 +71,7 @@
 #include <px4_platform_common/posix.h>
 #include <systemlib/mavlink_log.h>
 #include <systemlib/uthash/utlist.h>
-#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/radio_status.h>
@@ -258,7 +258,7 @@ public:
 
 	bool			get_forwarding_on() { return _forwarding_on; }
 
-	bool			is_connected() { return (hrt_elapsed_time(&_tstatus.heartbeat_time) < 3_s); }
+	bool			is_connected();
 
 #if defined(MAVLINK_UDP)
 	static Mavlink 		*get_instance_for_network_port(unsigned long port);
@@ -431,6 +431,9 @@ public:
 	 * Get the receive status of this MAVLink link
 	 */
 	telemetry_status_s	&get_telemetry_status() { return _tstatus; }
+	void                    lock_telemetry_status() { pthread_mutex_lock(&_telemetry_status_mutex); }
+	void                    unlock_telemetry_status() { pthread_mutex_unlock(&_telemetry_status_mutex); }
+	void                    telemetry_status_updated() { _tstatus_updated = true; }
 
 	void			set_telemetry_status_type(uint8_t type) { _tstatus.type = type; }
 
@@ -530,7 +533,7 @@ private:
 
 	orb_advert_t		_mavlink_log_pub{nullptr};
 
-	uORB::PublicationQueued<telemetry_status_s>	_telem_status_pub{ORB_ID(telemetry_status)};
+	uORB::PublicationMulti<telemetry_status_s> _telem_status_pub{ORB_ID(telemetry_status)};
 
 	bool			_task_running{true};
 	static bool		_boot_complete;
@@ -632,6 +635,7 @@ private:
 
 	radio_status_s		_rstatus {};
 	telemetry_status_s	_tstatus {};
+	bool                    _tstatus_updated{false};
 
 	ping_statistics_s	_ping_stats {};
 
@@ -646,6 +650,7 @@ private:
 
 	pthread_mutex_t		_message_buffer_mutex {};
 	pthread_mutex_t		_send_mutex {};
+	pthread_mutex_t		_telemetry_status_mutex {};
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,

src/modules/mavlink/mavlink_receiver.cpp

@@ -2045,25 +2045,61 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 {
 	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
 	if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) {
+
+		const hrt_abstime now = hrt_absolute_time();
+
 		mavlink_heartbeat_t hb;
 		mavlink_msg_heartbeat_decode(msg, &hb);
 
-		/* Accept only heartbeats from GCS or ONBOARD Controller, skip heartbeats from other vehicles */
+		const bool same_system = (msg->sysid == mavlink_system.sysid);
-		if ((msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) || (msg->sysid == mavlink_system.sysid
+
-				&& hb.type == MAV_TYPE_ONBOARD_CONTROLLER)) {
+		if (same_system || hb.type == MAV_TYPE_GCS) {
 
 			telemetry_status_s &tstatus = _mavlink->get_telemetry_status();
 
-			/* set heartbeat time and topic time and publish -
+			bool heartbeat_slot_found = false;
-			 * the telem status also gets updated on telemetry events
+			int heartbeat_slot = 0;
-			 */
-			tstatus.heartbeat_time = hrt_absolute_time();
-			tstatus.remote_system_id = msg->sysid;
-			tstatus.remote_component_id = msg->compid;
-			tstatus.remote_type = hb.type;
-			tstatus.remote_system_status = hb.system_status;
-		}
 
+			// find existing HEARTBEAT slot
+			for (size_t i = 0; i < (sizeof(tstatus.heartbeats) / sizeof(tstatus.heartbeats[0])); i++) {
+				if ((tstatus.heartbeats[i].system_id == msg->sysid)
+				    && (tstatus.heartbeats[i].component_id == msg->compid)
+				    && (tstatus.heartbeats[i].type == hb.type)) {
+
+					// found matching heartbeat slot
+					heartbeat_slot = i;
+					heartbeat_slot_found = true;
+					break;
+				}
+			}
+
+			// otherwise use first available slot
+			if (!heartbeat_slot_found) {
+				for (size_t i = 0; i < (sizeof(tstatus.heartbeats) / sizeof(tstatus.heartbeats[0])); i++) {
+					if ((tstatus.heartbeats[i].system_id == 0) && (tstatus.heartbeats[i].timestamp == 0)) {
+						heartbeat_slot = i;
+						heartbeat_slot_found = true;
+						break;
+					}
+				}
+			}
+
+			if (heartbeat_slot_found) {
+				_mavlink->lock_telemetry_status();
+
+				tstatus.heartbeats[heartbeat_slot].timestamp = now;
+				tstatus.heartbeats[heartbeat_slot].system_id = msg->sysid;
+				tstatus.heartbeats[heartbeat_slot].component_id = msg->compid;
+				tstatus.heartbeats[heartbeat_slot].type = hb.type;
+				tstatus.heartbeats[heartbeat_slot].state = hb.system_status;
+
+				_mavlink->telemetry_status_updated();
+				_mavlink->unlock_telemetry_status();
+
+			} else {
+				PX4_ERR("no telemetry heartbeat slots available");
+			}
+		}
 	}
 }