mavlink: telemetry status only log simple HEARTBEAT validity

* delete telemetry_heartbeat msg
 * delete unused _telemetry_status_mutex

msg/CMakeLists.txt

@@ -130,7 +130,6 @@ 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

@@ -1,51 +0,0 @@
-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

@@ -6,8 +6,6 @@ uint8 LINK_TYPE_IRIDIUM	= 4
 
 uint64 timestamp			# time since system start (microseconds)
 
-telemetry_heartbeat[4] heartbeats
-
 uint8 type				#  type of the radio hardware (LINK_TYPE_*)
 
 uint8 mode
@@ -27,3 +25,27 @@ float32 rate_rx
 
 float32 rate_tx
 float32 rate_txerr
+
+
+uint64 HEARTBEAT_TIMEOUT_US = 1500000       # Heartbeat timeout 1.5 seconds
+
+# Heartbeats per type
+bool heartbeat_type_antenna_tracker         # MAV_TYPE_ANTENNA_TRACKER
+bool heartbeat_type_gcs                     # MAV_TYPE_GCS
+bool heartbeat_type_onboard_controller      # MAV_TYPE_ONBOARD_CONTROLLER
+bool heartbeat_type_gimbal                  # MAV_TYPE_GIMBAL
+bool heartbeat_type_adsb                    # MAV_TYPE_ADSB
+bool heartbeat_type_camera                  # MAV_TYPE_CAMERA
+
+# Heartbeats per component
+bool heartbeat_component_telemetry_radio    # MAV_COMP_ID_TELEMETRY_RADIO
+bool heartbeat_component_log                # MAV_COMP_ID_LOG
+bool heartbeat_component_osd                # MAV_COMP_ID_OSD
+bool heartbeat_component_obstacle_avoidance # MAV_COMP_ID_OBSTACLE_AVOIDANCE
+bool heartbeat_component_vio                # MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY
+bool heartbeat_component_pairing_manager    # MAV_COMP_ID_PAIRING_MANAGER
+bool heartbeat_component_udp_bridge         # MAV_COMP_ID_UDP_BRIDGE
+bool heartbeat_component_uart_bridge        # MAV_COMP_ID_UART_BRIDGE
+
+# Misc component health
+bool avoidance_system_healthy

msg/tools/uorb_rtps_message_ids.yaml

@@ -277,18 +277,16 @@ rtps:
     id: 131
   - msg: yaw_estimator_status
     id: 132
-  - msg: telemetry_heartbeat
-    id: 133
   - msg: sensor_preflight_mag
-    id: 134
+    id: 133
   - msg: estimator_states
-    id: 135
+    id: 134
   - msg: generator_status
-    id: 136
+    id: 135
   - msg: sensor_gyro_fft
-    id: 137
+    id: 136
   - msg: navigator_mission_item
-    id: 138
+    id: 137
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

src/modules/commander/Commander.cpp

@@ -3809,65 +3809,47 @@ void Commander::data_link_check()
 				}
 			}
 
-			for (const auto &hb : telemetry.heartbeats) {
+			if (telemetry.heartbeat_type_gcs) {
-				// handle different remote types
-				switch (hb.type) {
-				case telemetry_heartbeat_s::TYPE_GCS:
-
 				// Initial connection or recovery from data link lost
 				if (status.data_link_lost) {
-						if (hb.timestamp > _datalink_last_heartbeat_gcs) {
 					status.data_link_lost = false;
 					_status_changed = true;
 
+					if (_datalink_last_heartbeat_gcs != 0) {
+						mavlink_log_info(&mavlink_log_pub, "Data link regained");
+					}
+
 					if (!armed.armed && !status_flags.condition_calibration_enabled) {
 						// make sure to report preflight check failures to a connecting GCS
 						PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags,
 									       _arm_requirements.global_position, true, true, hrt_elapsed_time(&_boot_timestamp));
 					}
-
-							if (_datalink_last_heartbeat_gcs != 0) {
-								mavlink_log_info(&mavlink_log_pub, "Data link regained");
-							}
-						}
 				}
 
-					// Only keep the very last heartbeat timestamp, so we don't get confused
+				_datalink_last_heartbeat_gcs = telemetry.timestamp;
-					// by multiple mavlink instances publishing different timestamps.
-					if (hb.timestamp > _datalink_last_heartbeat_gcs) {
-						_datalink_last_heartbeat_gcs = hb.timestamp;
 			}
 
-					break;
+			if (telemetry.heartbeat_type_onboard_controller) {
-
-				case telemetry_heartbeat_s::TYPE_ONBOARD_CONTROLLER:
 				if (_onboard_controller_lost) {
-						if (hb.timestamp > _datalink_last_heartbeat_onboard_controller) {
-							mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
 					_onboard_controller_lost = false;
 					_status_changed = true;
+
+					if (_datalink_last_heartbeat_onboard_controller != 0) {
+						mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
 					}
 				}
 
-					_datalink_last_heartbeat_onboard_controller = hb.timestamp;
+				_datalink_last_heartbeat_onboard_controller = telemetry.timestamp;
-
-					if (hb.component_id == telemetry_heartbeat_s::COMP_ID_OBSTACLE_AVOIDANCE) {
-						if (hb.timestamp > _datalink_last_heartbeat_avoidance_system) {
-							_avoidance_system_status_change = (_datalink_last_status_avoidance_system != hb.state);
 			}
 
-						_datalink_last_heartbeat_avoidance_system = hb.timestamp;
+			if (telemetry.heartbeat_component_obstacle_avoidance) {
-						_datalink_last_status_avoidance_system = hb.state;
-
 				if (_avoidance_system_lost) {
-							_status_changed = true;
 					_avoidance_system_lost = false;
-							status_flags.avoidance_system_valid = true;
+					_status_changed = true;
-						}
 				}
 
-					break;
+				_datalink_last_heartbeat_avoidance_system = telemetry.timestamp;
-				}
+				status_flags.avoidance_system_valid = telemetry.avoidance_system_healthy;
 			}
 		}
 	}
@@ -3899,40 +3881,15 @@ void Commander::data_link_check()
 
 	// AVOIDANCE SYSTEM state check (only if it is enabled)
 	if (status_flags.avoidance_system_required && !_onboard_controller_lost) {
-
+		// if heartbeats stop
-		//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_heartbeat_s::STATE_BOOT) {
-				status_flags.avoidance_system_valid = false;
 	}
 
-			if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_ACTIVE) {
-				status_flags.avoidance_system_valid = true;
-			}
-
-			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_heartbeat_s::STATE_FLIGHT_TERMINATION) {
-				status_flags.avoidance_system_valid = false;
-				_status_changed = true;
-			}
-
-			_avoidance_system_status_change = 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)) {

src/modules/commander/Commander.hpp

@@ -322,8 +322,6 @@ private:
 	hrt_abstime	_datalink_last_heartbeat_onboard_controller{0};
 	bool		_onboard_controller_lost{false};
 	bool		_avoidance_system_lost{false};
-	bool		_avoidance_system_status_change{false};
-	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};

src/modules/mavlink/mavlink_main.cpp

@@ -355,18 +355,6 @@ 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()
 {
@@ -2120,7 +2108,6 @@ 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) {
@@ -2524,7 +2511,6 @@ Mavlink::task_main(int argc, char *argv[])
 	}
 
 	pthread_mutex_destroy(&_send_mutex);
-	pthread_mutex_destroy(&_telemetry_status_mutex);
 
 	PX4_INFO("exiting channel %i", (int)_channel);
 
@@ -2618,11 +2604,9 @@ 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()
@@ -2761,10 +2745,8 @@ Mavlink::start(int argc, char *argv[])
 void
 Mavlink::display_status()
 {
-	for (const auto &hb : _tstatus.heartbeats) {
+	if (_tstatus.heartbeat_type_gcs) {
-		if ((hb.timestamp > 0) && (hb.type == telemetry_heartbeat_s::TYPE_GCS)) {
+		printf("\tGCS heartbeat valid\n");
-			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

@@ -258,7 +258,7 @@ public:
 
 	bool			get_forwarding_on() { return _forwarding_on; }
 
-	bool			is_connected();
+	bool			is_connected() { return _tstatus.heartbeat_type_gcs; }
 
 #if defined(MAVLINK_UDP)
 	static Mavlink 		*get_instance_for_network_port(unsigned long port);
@@ -430,9 +430,7 @@ public:
 	/**
 	 * Get the receive status of this MAVLink link
 	 */
-	telemetry_status_s	&get_telemetry_status() { return _tstatus; }
+	telemetry_status_s	&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; }
@@ -652,7 +650,6 @@ 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

@@ -2075,50 +2075,75 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 
 		if (same_system || hb.type == MAV_TYPE_GCS) {
 
-			telemetry_status_s &tstatus = _mavlink->get_telemetry_status();
+			switch (hb.type) {
-
+			case MAV_TYPE_ANTENNA_TRACKER:
-			bool heartbeat_slot_found = false;
+				_heartbeat_type_antenna_tracker = now;
-			int heartbeat_slot = 0;
-
-			// 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
+			case MAV_TYPE_GCS:
-			if (!heartbeat_slot_found) {
+				_heartbeat_type_gcs = now;
-				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;
-					}
+
-				}
+			case MAV_TYPE_ONBOARD_CONTROLLER:
+				_heartbeat_type_onboard_controller = now;
+				break;
+
+			case MAV_TYPE_GIMBAL:
+				_heartbeat_type_gimbal = now;
+				break;
+
+			case MAV_TYPE_ADSB:
+				_heartbeat_type_adsb = now;
+				break;
+
+			case MAV_TYPE_CAMERA:
+				_heartbeat_type_camera = now;
+				break;
+
+			default:
+				PX4_DEBUG("unhandled HEARTBEAT MAV_TYPE: %d from SYSID: %d, COMPID: %d", hb.type, msg->sysid, msg->compid);
 			}
 
-			if (heartbeat_slot_found) {
-				_mavlink->lock_telemetry_status();
 
-				tstatus.heartbeats[heartbeat_slot].timestamp = now;
+			switch (msg->compid) {
-				tstatus.heartbeats[heartbeat_slot].system_id = msg->sysid;
+			case MAV_COMP_ID_TELEMETRY_RADIO:
-				tstatus.heartbeats[heartbeat_slot].component_id = msg->compid;
+				_heartbeat_component_telemetry_radio = now;
-				tstatus.heartbeats[heartbeat_slot].type = hb.type;
+				break;
-				tstatus.heartbeats[heartbeat_slot].state = hb.system_status;
 
-				_mavlink->telemetry_status_updated();
+			case MAV_COMP_ID_LOG:
-				_mavlink->unlock_telemetry_status();
+				_heartbeat_component_log = now;
+				break;
 
-			} else {
+			case MAV_COMP_ID_OSD:
-				PX4_ERR("no telemetry heartbeat slots available");
+				_heartbeat_component_osd = now;
+				break;
+
+			case MAV_COMP_ID_OBSTACLE_AVOIDANCE:
+				_heartbeat_component_obstacle_avoidance = now;
+				_mavlink->telemetry_status().avoidance_system_healthy = (hb.system_status == MAV_STATE_ACTIVE);
+				break;
+
+			case MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY:
+				_heartbeat_component_visual_inertial_odometry = now;
+				break;
+
+			case MAV_COMP_ID_PAIRING_MANAGER:
+				_heartbeat_component_pairing_manager = now;
+				break;
+
+			case MAV_COMP_ID_UDP_BRIDGE:
+				_heartbeat_component_udp_bridge = now;
+				break;
+
+			case MAV_COMP_ID_UART_BRIDGE:
+				_heartbeat_component_uart_bridge = now;
+				break;
+
+			default:
+				PX4_DEBUG("unhandled HEARTBEAT MAV_TYPE: %d from SYSID: %d, COMPID: %d", hb.type, msg->sysid, msg->compid);
 			}
+
+			CheckHeartbeats(now, true);
 		}
 	}
 }
@@ -2825,6 +2850,39 @@ void MavlinkReceiver::handle_message_statustext(mavlink_message_t *msg)
 	}
 }
 
+void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
+{
+	// check HEARTBEATs for timeout
+	static constexpr uint64_t TIMEOUT = telemetry_status_s::HEARTBEAT_TIMEOUT_US;
+
+	if (t <= TIMEOUT) {
+		return;
+	}
+
+	if ((t >= _last_heartbeat_check + (TIMEOUT / 2)) || force) {
+		telemetry_status_s &tstatus = _mavlink->telemetry_status();
+
+		tstatus.heartbeat_type_antenna_tracker         = (t <= TIMEOUT + _heartbeat_type_antenna_tracker);
+		tstatus.heartbeat_type_gcs                     = (t <= TIMEOUT + _heartbeat_type_gcs);
+		tstatus.heartbeat_type_onboard_controller      = (t <= TIMEOUT + _heartbeat_type_onboard_controller);
+		tstatus.heartbeat_type_gimbal                  = (t <= TIMEOUT + _heartbeat_type_gimbal);
+		tstatus.heartbeat_type_adsb                    = (t <= TIMEOUT + _heartbeat_type_adsb);
+		tstatus.heartbeat_type_camera                  = (t <= TIMEOUT + _heartbeat_type_camera);
+
+		tstatus.heartbeat_component_telemetry_radio    = (t <= TIMEOUT + _heartbeat_component_telemetry_radio);
+		tstatus.heartbeat_component_log                = (t <= TIMEOUT + _heartbeat_component_log);
+		tstatus.heartbeat_component_osd                = (t <= TIMEOUT + _heartbeat_component_osd);
+		tstatus.heartbeat_component_obstacle_avoidance = (t <= TIMEOUT + _heartbeat_component_obstacle_avoidance);
+		tstatus.heartbeat_component_vio                = (t <= TIMEOUT + _heartbeat_component_visual_inertial_odometry);
+		tstatus.heartbeat_component_pairing_manager    = (t <= TIMEOUT + _heartbeat_component_pairing_manager);
+		tstatus.heartbeat_component_udp_bridge         = (t <= TIMEOUT + _heartbeat_component_udp_bridge);
+		tstatus.heartbeat_component_uart_bridge        = (t <= TIMEOUT + _heartbeat_component_uart_bridge);
+
+		_mavlink->telemetry_status_updated();
+		_last_heartbeat_check = t;
+	}
+}
+
 /**
  * Receive data from UART/UDP
  */
@@ -2993,7 +3051,9 @@ MavlinkReceiver::Run()
 			usleep(10000);
 		}
 
-		hrt_abstime t = hrt_absolute_time();
+		const hrt_abstime t = hrt_absolute_time();
+
+		CheckHeartbeats(t);
 
 		if (t - last_send_update > timeout * 1000) {
 			_mission_manager.check_active_mission();

src/modules/mavlink/mavlink_receiver.h

@@ -182,6 +182,7 @@ private:
 	void handle_message_utm_global_position(mavlink_message_t *msg);
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
 
+	void CheckHeartbeats(const hrt_abstime &t, bool force = false);
 
 	void Run();
 
@@ -314,6 +315,24 @@ private:
 	// Allocated if needed.
 	TunePublisher *_tune_publisher{nullptr};
 
+	hrt_abstime _last_heartbeat_check{0};
+
+	hrt_abstime _heartbeat_type_antenna_tracker{0};
+	hrt_abstime _heartbeat_type_gcs{0};
+	hrt_abstime _heartbeat_type_onboard_controller{0};
+	hrt_abstime _heartbeat_type_gimbal{0};
+	hrt_abstime _heartbeat_type_adsb{0};
+	hrt_abstime _heartbeat_type_camera{0};
+
+	hrt_abstime _heartbeat_component_telemetry_radio{0};
+	hrt_abstime _heartbeat_component_log{0};
+	hrt_abstime _heartbeat_component_osd{0};
+	hrt_abstime _heartbeat_component_obstacle_avoidance{0};
+	hrt_abstime _heartbeat_component_visual_inertial_odometry{0};
+	hrt_abstime _heartbeat_component_pairing_manager{0};
+	hrt_abstime _heartbeat_component_udp_bridge{0};
+	hrt_abstime _heartbeat_component_uart_bridge{0};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::BAT_CRIT_THR>)     _param_bat_crit_thr,
 		(ParamFloat<px4::params::BAT_EMERGEN_THR>)  _param_bat_emergen_thr,