HEARTBEAT and commander failsafe handling cleanup

msg/telemetry_status.msg

@@ -1,18 +1,41 @@
-uint64 timestamp			# time since system start (microseconds)
+uint8 LINK_TYPE_GENERIC = 0
+uint8 LINK_TYPE_3DR_RADIO = 1
+uint8 LINK_TYPE_UBIQUITY_BULLET = 2
+uint8 LINK_TYPE_WIRE = 3
+uint8 LINK_TYPE_USB = 4
+uint8 LINK_TYPE_IRIDIUM	= 5
 
-uint8 TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0
+# MAV_COMPONENT (fill in as needed)
-uint8 TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1
+uint8 COMPONENT_ID_ALL = 0
-uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2
+uint8 COMPONENT_ID_AUTOPILOT1 = 1
-uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3
+uint8 COMPONENT_ID_CAMERA = 100
-uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4
+
-uint8 TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM = 5
+# 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
 
-uint8 type				# type of the radio hardware
+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 system_id				# system id of the remote system
+uint8 type				# type of the radio hardware
-uint8 component_id			# component id of the remote system
 
 uint8 mode
 

msg/vehicle_status.msg

@@ -62,6 +62,7 @@ uint8 system_id			# system id, contains MAVLink's system ID field
 uint8 component_id			# subsystem / component id, contains MAVLink's component ID field
 
 bool is_rotary_wing			# True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter
+
 bool is_vtol				# True if the system is VTOL capable
 bool vtol_fw_permanent_stab		# True if VTOL should stabilize attitude for fw in manual mode
 bool in_transition_mode			# True if VTOL is doing a transition
@@ -71,10 +72,13 @@ bool rc_signal_lost				# true if RC reception lost
 uint8 rc_input_mode				# set to 1 to disable the RC input, 2 to enable manual control to RC in mapping.
 
 bool data_link_lost				# datalink to GCS lost
-bool high_latency_data_link_active 		# all low latency datalinks to GCS lost
 uint8 data_link_lost_counter			# counts unique data link lost events
+
+bool high_latency_data_link_lost 			# all low latency datalinks to GCS lost
+
 bool engine_failure				# Set to true if an engine failure is detected
 bool mission_failure				# Set to true if mission could not continue/finish
+
 uint8 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
 
 # see SYS_STATUS mavlink message for the following

src/modules/commander/Commander.cpp

@@ -559,11 +559,42 @@ Commander::Commander() :
 	_auto_disarm_landed.set_hysteresis_time_from(false, 10_s);
 	_auto_disarm_killed.set_hysteresis_time_from(false, 5_s);
 	_battery_sub = orb_subscribe(ORB_ID(battery_status));
+
+	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		_telemetry_status_sub[i] = -1;
+	}
+
+	// We want to accept RC inputs as default
+	status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
+	internal_state.main_state = commander_state_s::MAIN_STATE_MANUAL;
+	internal_state.timestamp = hrt_absolute_time();
+	status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
+	status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
+
+	/* mark all signals lost as long as they haven't been found */
+	status.rc_signal_lost = true;
+	status_flags.offboard_control_signal_lost = true;
+	status.data_link_lost = true;
+
+	status.timestamp = hrt_absolute_time();
+
+	status_flags.condition_power_input_valid = true;
+	status_flags.rc_calibration_valid = true;
 }
 
 Commander::~Commander()
 {
 	orb_unsubscribe(_battery_sub);
+
+	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		if (_telemetry_status_sub[i] >= 0) {
+			orb_unsubscribe(_telemetry_status_sub[i]);
+		}
+	}
+
+	if (_iridiumsbd_status_sub >= 0) {
+		orb_unsubscribe(_iridiumsbd_status_sub);
+	}
 }
 
 bool
@@ -1004,16 +1035,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: {
-			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) {
+			if (_high_latency_datalink_heartbeat > commander_boot_timestamp) {
 				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED;
 				mavlink_log_critical(&mavlink_log_pub, "Control high latency failed, no hl telemetry available");
 			}
@@ -1161,15 +1184,8 @@ Commander::run()
 	param_t _param_sys_type = param_find("MAV_TYPE");
 	param_t _param_system_id = param_find("MAV_SYS_ID");
 	param_t _param_component_id = param_find("MAV_COMP_ID");
-	param_t _param_enable_datalink_loss = param_find("NAV_DLL_ACT");
 	param_t _param_offboard_loss_act = param_find("COM_OBL_ACT");
 	param_t _param_offboard_loss_rc_act = param_find("COM_OBL_RC_ACT");
-	param_t _param_enable_rc_loss = param_find("NAV_RCL_ACT");
-	param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
-	param_t _param_highlatencydatalink_loss_timeout = param_find("COM_HLDL_LOSS_T");
-	param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T");
-	param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T");
-	param_t _param_highlatencydatalink_regain_timeout = param_find("COM_HLDL_REG_T");
 	param_t _param_ef_throttle_thres = param_find("COM_EF_THROT");
 	param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T");
 	param_t _param_ef_time_thres = param_find("COM_EF_TIME");
@@ -1225,42 +1241,16 @@ Commander::run()
 		PX4_ERR("Failed to register power notification callback");
 	}
 
-	// We want to accept RC inputs as default
-	status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
-	internal_state.main_state = commander_state_s::MAIN_STATE_MANUAL;
-	internal_state.timestamp = hrt_absolute_time();
-	status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
-	status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
-
-	/* mark all signals lost as long as they haven't been found */
-	status.rc_signal_lost = true;
-	status_flags.offboard_control_signal_lost = true;
-	status.data_link_lost = true;
-
-	status.timestamp = hrt_absolute_time();
-
-	status_flags.condition_power_input_valid = true;
-	status_flags.rc_calibration_valid = true;
-
 	get_circuit_breaker_params();
 
 	/* publish initial state */
 	_status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
 
-	if (_status_pub == nullptr) {
-		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
-		warnx("exiting.");
-		px4_task_exit(PX4_ERROR);
-	}
-
-	/* Initialize armed with all false */
-	memset(&armed, 0, sizeof(armed));
 	/* armed topic */
 	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
 	hrt_abstime last_disarmed_timestamp = 0;
 
 	/* vehicle control mode topic */
-	memset(&control_mode, 0, sizeof(control_mode));
 	orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
 
 	/* command ack */
@@ -1281,54 +1271,24 @@ Commander::run()
 
 	bool updated = false;
 
-	/* Subscribe to safety topic */
+	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	int safety_sub = orb_subscribe(ORB_ID(safety));
+	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-	memset(&safety, 0, sizeof(safety));
+	int cpuload_sub = orb_subscribe(ORB_ID(cpuload));
-	safety.safety_switch_available = false;
-	safety.safety_off = false;
-
-	/* Subscribe to geofence result topic */
 	int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result));
-	struct geofence_result_s geofence_result;
-	memset(&geofence_result, 0, sizeof(geofence_result));
-
-	/* Subscribe to manual control data */
-	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	memset(&sp_man, 0, sizeof(sp_man));
-
-	/* Subscribe to offboard control data */
-	int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode));
-	memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));
-
-	/* Subscribe to land detector */
 	int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
+	int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode));
+	int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
+	int safety_sub = orb_subscribe(ORB_ID(safety));
+	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
+	int system_power_sub = orb_subscribe(ORB_ID(system_power));
+	int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
+
+	struct geofence_result_s geofence_result {};
+
 	land_detector.landed = true;
 
-	/* Subscribe to command topic */
+	vtol_status.vtol_in_rw_mode = true;		// default for vtol is rotary wing
-	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-
-	/* Subscribe to parameters changed topic */
-	int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
-
-	/* Subscribe to subsystem info topic */
-	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
-	struct subsystem_info_s info;
-	memset(&info, 0, sizeof(info));
-
-	/* Subscribe to system power */
-	int system_power_sub = orb_subscribe(ORB_ID(system_power));
-
-	/* Subscribe to actuator controls (outputs) */
-	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-
-	/* Subscribe to vtol vehicle status topic */
-	int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
-	//struct vtol_vehicle_status_s vtol_status;
-	memset(&vtol_status, 0, sizeof(vtol_status));
-	vtol_status.vtol_in_rw_mode = true;		//default for vtol is rotary wing
-
-	int cpuload_sub = orb_subscribe(ORB_ID(cpuload));
-	memset(&cpuload, 0, sizeof(cpuload));
 
 	control_status_leds(&status, &armed, true, _battery_warning, &cpuload);
 
@@ -1371,20 +1331,11 @@ Commander::run()
 	param_get(_param_rc_arm_hyst, &rc_arm_hyst);
 	rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC;
 
-	int32_t datalink_loss_act = 0;
-	int32_t rc_loss_act = 0;
-	int32_t datalink_loss_timeout = 10;
-	int32_t highlatencydatalink_loss_timeout = 120;
-	float rc_loss_timeout = 0.5;
-	int32_t datalink_regain_timeout = 0;
-	int32_t highlatencydatalink_regain_timeout = 0;
 	float offboard_loss_timeout = 0.0f;
 	int32_t offboard_loss_act = 0;
 	int32_t offboard_loss_rc_act = 0;
 	int32_t posctl_nav_loss_act = 0;
-
 	int32_t geofence_action = 0;
-
 	int32_t flight_uuid = 0;
 	int32_t airmode = 0;
 	int32_t rc_map_arm_switch = 0;
@@ -1479,11 +1430,6 @@ Commander::run()
 			}
 
 			/* Safety parameters */
-			param_get(_param_enable_datalink_loss, &datalink_loss_act);
-			param_get(_param_enable_rc_loss, &rc_loss_act);
-			param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
-			param_get(_param_highlatencydatalink_loss_timeout, &highlatencydatalink_loss_timeout);
-			param_get(_param_rc_loss_timeout, &rc_loss_timeout);
 			param_get(_param_rc_in_off, &rc_in_off);
 			status.rc_input_mode = rc_in_off;
 			param_get(_param_rc_arm_hyst, &rc_arm_hyst);
@@ -1492,8 +1438,6 @@ Commander::run()
 			// percentage (* 0.01) needs to be doubled because RC total interval is 2, not 1
 			min_stick_change *= 0.02f;
 			rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC;
-			param_get(_param_datalink_regain_timeout, &datalink_regain_timeout);
-			param_get(_param_highlatencydatalink_regain_timeout, &highlatencydatalink_regain_timeout);
 			param_get(_param_ef_throttle_thres, &ef_throttle_thres);
 			param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
 			param_get(_param_ef_time_thres, &ef_time_thres);
@@ -1594,9 +1538,6 @@ Commander::run()
 			}
 		}
 
-		// poll the telemetry status
-		poll_telemetry_status();
-
 		orb_check(system_power_sub, &updated);
 
 		if (updated) {
@@ -1800,6 +1741,7 @@ Commander::run()
 			orb_check(subsys_sub, &updated);
 
 			if (updated) {
+				subsystem_info_s info{};
 				orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
 				set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status);
 				status_changed = true;
@@ -2003,7 +1945,7 @@ Commander::run()
 
 		/* RC input check */
 		if (!status_flags.rc_input_blocked && sp_man.timestamp != 0 &&
-		    (hrt_elapsed_time(&sp_man.timestamp) < (rc_loss_timeout * 1_s))) {
+		    (hrt_elapsed_time(&sp_man.timestamp) < (_rc_loss_threshold.get() * 1_s))) {
 
 			/* handle the case where RC signal was regained */
 			if (!status_flags.rc_signal_found_once) {
@@ -2180,8 +2122,7 @@ Commander::run()
 		}
 
 		// data link checks which update the status
-		data_link_checks(highlatencydatalink_loss_timeout, highlatencydatalink_regain_timeout, datalink_loss_timeout,
+		data_link_check(status_changed);
-				 datalink_regain_timeout, &status_changed);
 
 		// engine failure detection
 		// TODO: move out of commander
@@ -2421,12 +2362,12 @@ Commander::run()
 						       &armed,
 						       &internal_state,
 						       &mavlink_log_pub,
-						       (link_loss_actions_t)datalink_loss_act,
+						       (link_loss_actions_t)_datalink_loss_action.get(),
 						       _mission_result_sub.get().finished,
 						       _mission_result_sub.get().stay_in_failsafe,
 						       status_flags,
 						       land_detector.landed,
-						       (link_loss_actions_t)rc_loss_act,
+						       (link_loss_actions_t)_rc_loss_action.get(),
 						       offboard_loss_act,
 						       offboard_loss_rc_act,
 						       posctl_nav_loss_act);
@@ -3888,203 +3829,121 @@ bool Commander::preflight_check(bool report)
 	return success;
 }
 
-void Commander::poll_telemetry_status()
+void Commander::data_link_check(bool &status_changed)
 {
-	bool updated = false;
-
 	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
 
-		if (_telemetry[i].subscriber < 0) {
+		if (_telemetry_status_sub[i] < 0) {
-			_telemetry[i].subscriber = orb_subscribe_multi(ORB_ID(telemetry_status), i);
+			if (orb_exists(ORB_ID(telemetry_status), i) == PX4_OK) {
+				_telemetry_status_sub[i] = orb_subscribe_multi(ORB_ID(telemetry_status), i);
+			}
 		}
 
-		orb_check(_telemetry[i].subscriber, &updated);
+		bool updated = false;
+
+		orb_check(_telemetry_status_sub[i], &updated);
 
 		if (updated) {
-			telemetry_status_s telemetry = {};
+			telemetry_status_s telemetry;
-			orb_copy(ORB_ID(telemetry_status), _telemetry[i].subscriber, &telemetry);
 
-			/* perform system checks when new telemetry link connected */
+			if (orb_copy(ORB_ID(telemetry_status), _telemetry_status_sub[i], &telemetry) == PX4_OK) {
-			if (/* we first connect a link or re-connect a link after loosing it or haven't yet reported anything */
-				(_telemetry[i].last_heartbeat == 0 || (hrt_elapsed_time(&_telemetry[i].last_heartbeat) > 3_s)
-				 || !_telemetry[i].preflight_checks_reported) &&
-				/* and this link has a communication partner */
-				(telemetry.heartbeat_time > 0) &&
-				/* and it is still connected */
-				(hrt_elapsed_time(&telemetry.heartbeat_time) < 2_s) &&
-				/* and the system is not already armed (and potentially flying) */
-				!armed.armed) {
 
-				/* flag the checks as reported for this link when we actually report them */
+				// handle different radio types
-				_telemetry[i].preflight_checks_reported = status_flags.condition_system_hotplug_timeout;
+				switch (telemetry.type) {
+				case (telemetry_status_s::LINK_TYPE_USB):
+					// set (but don't unset) telemetry via USB as active once a MAVLink connection is up
+					status_flags.usb_connected = true;
+					break;
 
-				preflight_check(true);
+				case (telemetry_status_s::LINK_TYPE_IRIDIUM):
 
-				// Provide feedback on mission state
+					// lazily subscribe
-				const mission_result_s &mission_result = _mission_result_sub.get();
+					if (orb_exists(ORB_ID(iridiumsbd_status), 0) == PX4_OK) {
+						_iridiumsbd_status_sub = orb_subscribe(ORB_ID(iridiumsbd_status));
+					}
 
-				if ((mission_result.timestamp > commander_boot_timestamp) && status_flags.condition_system_hotplug_timeout &&
+					if (_iridiumsbd_status_sub >= 0) {
-				    (mission_result.instance_count > 0) && !mission_result.valid) {
+						bool iridiumsbd_updated = false;
+						orb_check(_iridiumsbd_status_sub, &iridiumsbd_updated);
 
-					mavlink_log_critical(&mavlink_log_pub, "Planned mission fails check. Please upload again.");
+						if (iridiumsbd_updated) {
-					//set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, false, status); // TODO
+							iridiumsbd_status_s iridium_status;
 
-				} else {
+							if (orb_copy(ORB_ID(iridiumsbd_status), _iridiumsbd_status_sub, &iridium_status) == PX4_OK) {
-					//set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, true, status); // TODO
+								_high_latency_datalink_heartbeat = iridium_status.last_heartbeat;
+
+								if (status.high_latency_data_link_lost) {
+									if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_high_latency_datalink_regain_threshold.get() * 1_s)) {
+										status.high_latency_data_link_lost = false;
+										status_changed = true;
+									}
+								}
+
+							}
+						}
+					}
+
+					break;
 				}
-			}
 
-			/* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */
-			if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) {
-				status_flags.usb_connected = true;
-			}
 
-			/* set latency type of the telemetry connection */
+				// handle different remote types
-			if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM) {
+				switch (telemetry.remote_type) {
-				_telemetry[i].high_latency = true;
+				case (telemetry_status_s::MAV_TYPE_GCS):
+					_datalink_last_heartbeat_gcs = telemetry.heartbeat_time;
 
-			} else {
+					if (status.data_link_lost) {
-				_telemetry[i].high_latency = false;
+						if (hrt_elapsed_time(&_datalink_lost) > (_datalink_regain_threshold.get() * 1_s)) {
-			}
+							status.data_link_lost = false;
+							status_changed = true;
+						}
+					}
 
-			if (telemetry.heartbeat_time > 0 && (_telemetry[i].last_heartbeat < telemetry.heartbeat_time)) {
+					break;
-				_telemetry[i].last_heartbeat = telemetry.heartbeat_time;
-			}
-		}
 
-		// for iridium telemetry use the iridiumsbd_status to update the heartbeat
+				case (telemetry_status_s::MAV_TYPE_ONBOARD_CONTROLLER):
-		if (_telemetry[i].high_latency) {
+					_datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;
-			if (_iridiumsbd_status_sub.update()) {
+					// TODO: FYI @baumanta
-				const hrt_abstime isbd_timestamp = _iridiumsbd_status_sub.get().last_heartbeat;
+					break;
-
-				if (isbd_timestamp > _telemetry[i].last_heartbeat) {
-					_telemetry[i].last_heartbeat = isbd_timestamp;
 				}
 			}
 		}
 	}
-}
 
-void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32_t highlatencydatalink_regain_timeout,
+	// GCS data link loss failsafe
-				 int32_t datalink_loss_timeout, int32_t datalink_regain_timeout, bool *status_changed)
+	if ((_datalink_last_heartbeat_gcs > 0) &&
-{
+	    (hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_datalink_loss_threshold.get() * 1_s))) {
-	/* data links check */
-	bool have_link = false;
-	bool high_latency_link_exists = false;
-	bool have_low_latency_link = false;
-	int32_t dl_loss_timeout_local = 0;
-	int32_t dl_regain_timeout_local = 0;
 
-	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		_datalink_lost = hrt_absolute_time();
-		if (_telemetry[i].high_latency) {
-			high_latency_link_exists = true;
-
-			if (status.high_latency_data_link_active) {
-				dl_loss_timeout_local = highlatencydatalink_loss_timeout;
-				dl_regain_timeout_local = highlatencydatalink_regain_timeout;
-
-			} else {
-				// if the high latency link is inactive we do not want to accidentally detect it as an active link
-				dl_loss_timeout_local = INT32_MIN;
-				dl_regain_timeout_local = INT32_MAX;
-			}
-
-		} else {
-			dl_loss_timeout_local = datalink_loss_timeout;
-			dl_regain_timeout_local = datalink_regain_timeout;
-		}
-
-		if (_telemetry[i].last_heartbeat != 0 &&
-		    hrt_elapsed_time(&_telemetry[i].last_heartbeat) < dl_loss_timeout_local * 1e6) {
-			/* handle the case where data link was gained first time or regained,
-			 * accept datalink as healthy only after datalink_regain_timeout seconds
-			 * */
-			if (_telemetry[i].lost &&
-			    hrt_elapsed_time(&_telemetry[i].last_dl_loss) > dl_regain_timeout_local * 1e6) {
-
-				/* report a regain */
-				if (_telemetry[i].last_dl_loss > 0) {
-					mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i regained", i);
-
-				} else if (_telemetry[i].last_dl_loss == 0) {
-					/* new link */
-				}
-
-				/* got link again or new */
-				*status_changed = true;
-
-				_telemetry[i].lost = false;
-				have_link = true;
-
-				if (!_telemetry[i].high_latency) {
-					have_low_latency_link = true;
-				}
-
-			} else if (!_telemetry[i].lost) {
-				/* telemetry was healthy also in last iteration
-				 * we don't have to check a timeout */
-				have_link = true;
-
-				if (!_telemetry[i].high_latency) {
-					have_low_latency_link = true;
-				}
-			}
-
-		} else {
-
-			if (!_telemetry[i].lost) {
-				/* only reset the timestamp to a different time on state change */
-				_telemetry[i].last_dl_loss  = hrt_absolute_time();
-
-				mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i lost", i);
-				_telemetry[i].lost = true;
-			}
-		}
-	}
-
-	if (have_link) {
-		/* handle the case where data link was regained */
-		if (status.data_link_lost) {
-			status.data_link_lost = false;
-			*status_changed = true;
-		}
-
-		if (status.high_latency_data_link_active && have_low_latency_link) {
-			// regained a low latency telemetry link, deactivate the high latency link
-			// to avoid transmitting unnecessary data over that link
-			status.high_latency_data_link_active = false;
-			*status_changed = true;
-			mavlink_log_critical(&mavlink_log_pub, "LOW LAT LINKS REGAINED, DEACTIVATE HIGH LAT LINK");
-		}
-
-	} else {
-		if (high_latency_link_exists && !status.high_latency_data_link_active && armed.armed) {
-			// low latency telemetry lost and high latency link existing
-			status.high_latency_data_link_active = true;
-			*status_changed = true;
-
-			// 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) {
-					_telemetry[i].last_heartbeat = hrt_absolute_time();
-				}
-			}
-
-			if (!status.data_link_lost) {
-				mavlink_log_critical(&mavlink_log_pub, "ALL LOW LAT LINKS LOST, ACTIVATE HIGH LAT LINK");
-
-			} else {
-				mavlink_log_critical(&mavlink_log_pub, "ACTIVATING AVAILABLE HIGH LATENCY LINK");
-			}
-
-		} else if (!status.data_link_lost) {
-			if (armed.armed) {
-				mavlink_log_critical(&mavlink_log_pub, "ALL DATA LINKS LOST");
-			}
 
+		if (!status.data_link_lost) {
 			status.data_link_lost = true;
 			status.data_link_lost_counter++;
-			*status_changed = true;
+
+			mavlink_log_critical(&mavlink_log_pub, "DATA LINK LOST");
+
+			status_changed = true;
+		}
+	}
+
+	// ONBOARD CONTROLLER data link loss failsafe (hard coded 5 seconds)
+	//  only issue a periodic warning for now
+	if ((_datalink_last_heartbeat_onboard_controller > 0)
+	    && (hrt_elapsed_time(&_datalink_last_heartbeat_onboard_controller) > 5_s) &&
+	    (hrt_elapsed_time(&_onboard_controller_lost) > 5_s)) {
+
+		_onboard_controller_lost = hrt_absolute_time();
+		mavlink_log_critical(&mavlink_log_pub, "ONBOARD CONTROLLER LOST");
+	}
+
+	// high latency data link loss failsafe
+	if (hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_high_latency_datalink_loss_threshold.get() * 1_s)) {
+		_high_latency_datalink_lost = hrt_absolute_time();
+
+		if (!status.high_latency_data_link_lost) {
+			status.high_latency_data_link_lost = true;
+			mavlink_log_critical(&mavlink_log_pub, "HIGH LATENCY DATA LINK LOST");
+			status_changed = true;
 		}
 	}
 }

src/modules/commander/Commander.hpp

@@ -93,6 +93,17 @@ public:
 private:
 
 	DEFINE_PARAMETERS(
+
+		(ParamInt<px4::params::NAV_DLL_ACT>) _datalink_loss_action,
+		(ParamInt<px4::params::COM_DL_LOSS_T>) _datalink_loss_threshold,
+		(ParamInt<px4::params::COM_DL_REG_T>) _datalink_regain_threshold,
+
+		(ParamInt<px4::params::COM_HLDL_LOSS_T>) _high_latency_datalink_loss_threshold,
+		(ParamInt<px4::params::COM_HLDL_REG_T>) _high_latency_datalink_regain_threshold,
+
+		(ParamInt<px4::params::NAV_RCL_ACT>) _rc_loss_action,
+		(ParamFloat<px4::params::COM_RC_LOSS_T>) _rc_loss_threshold,
+
 		(ParamFloat<px4::params::COM_HOME_H_T>) _home_eph_threshold,
 		(ParamFloat<px4::params::COM_HOME_V_T>) _home_epv_threshold,
 
@@ -155,27 +166,21 @@ private:
 
 	void mission_init();
 
-	/**
-	 * Update the telemetry status and the corresponding status variables.
-	 * Perform system checks when new telemetry link connected.
-	 */
-	void poll_telemetry_status();
-
 	/**
 	 * Checks the status of all available data links and handles switching between different system telemetry states.
 	 */
-	void data_link_checks(int32_t highlatencydatalink_loss_timeout, int32_t highlatencydatalink_regain_timeout,
+	void		data_link_check(bool &status_changed);
-			      int32_t datalink_loss_timeout, int32_t datalink_regain_timeout, bool *status_changed);
+	int		_telemetry_status_sub[ORB_MULTI_MAX_INSTANCES] {};
 
-	// telemetry variables
+	uint64_t	_datalink_last_heartbeat_gcs{0};
-	struct telemetry_data {
+	uint64_t	_datalink_lost{0};
-		int subscriber = -1;
+
-		uint64_t last_heartbeat = 0u;
+	uint64_t	_datalink_last_heartbeat_onboard_controller{0};
-		uint64_t last_dl_loss = 0u;
+	uint64_t	_onboard_controller_lost{0};
-		bool preflight_checks_reported = false;
+
-		bool lost = true;
+	int		_iridiumsbd_status_sub{-1};
-		bool high_latency = false;
+	uint64_t	_high_latency_datalink_heartbeat{0};
-	} _telemetry[ORB_MULTI_MAX_INSTANCES];
+	uint64_t	_high_latency_datalink_lost{0};
 
 	void estimator_check(bool *status_changed);
 
@@ -190,7 +195,6 @@ private:
 
 	// Subscriptions
 	Subscription<estimator_status_s>		_estimator_status_sub{ORB_ID(estimator_status)};
-	Subscription<iridiumsbd_status_s> 		_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
 	Subscription<mission_result_s>			_mission_result_sub{ORB_ID(mission_result)};
 	Subscription<vehicle_global_position_s>		_global_position_sub{ORB_ID(vehicle_global_position)};
 	Subscription<vehicle_local_position_s>		_local_position_sub{ORB_ID(vehicle_local_position)};

src/modules/commander/commander_params.c

@@ -769,3 +769,45 @@ PARAM_DEFINE_INT32(COM_FLIGHT_UUID, 0);
  * @group Mission
  */
 PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0);
+
+/**
+ * Set data link loss failsafe mode
+ *
+ * The data link loss failsafe will only be entered after a timeout,
+ * set by COM_DL_LOSS_T in seconds. Once the timeout occurs the selected
+ * action will be executed. Setting this parameter to 4 will enable CASA
+ * Outback Challenge rules, which are only recommended to participants
+ * of that competition.
+ *
+ * @value 0 Disabled
+ * @value 1 Hold mode
+ * @value 2 Return mode
+ * @value 3 Land mode
+ * @value 4 Data Link Auto Recovery (CASA Outback Challenge rules)
+ * @value 5 Terminate
+ * @value 6 Lockdown
+ *
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
+
+/**
+ * Set RC loss failsafe mode
+ *
+ * The RC loss failsafe will only be entered after a timeout,
+ * set by COM_RC_LOSS_T in seconds. If RC input checks have been disabled
+ * by setting the COM_RC_IN_MODE param it will not be triggered.
+ * Setting this parameter to 4 will enable CASA Outback Challenge rules,
+ * which are only recommended to participants of that competition.
+ *
+ * @value 0 Disabled
+ * @value 1 Hold mode
+ * @value 2 Return mode
+ * @value 3 Land mode
+ * @value 4 RC Auto Recovery (CASA Outback Challenge rules)
+ * @value 5 Terminate
+ * @value 6 Lockdown
+ *
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);

src/modules/commander/state_machine_helper.cpp

@@ -834,48 +834,63 @@ void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
 			     uint8_t auto_recovery_nav_state)
 {
 	// do the best you can according to the action set
-	if (link_loss_act == link_loss_actions_t::AUTO_RECOVER
-	    && status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
-		status->nav_state = auto_recovery_nav_state;
 
-	} else if (link_loss_act == link_loss_actions_t::AUTO_LOITER && status_flags.condition_global_position_valid) {
+	switch (link_loss_act) {
-		status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+	case (link_loss_actions_t::DISABLED):
-
+	case (link_loss_actions_t::AUTO_RECOVER):
-	} else if (link_loss_act == link_loss_actions_t::AUTO_RTL
+		if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
-		   && status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
+			status->nav_state = auto_recovery_nav_state;
-
+			return;
-		main_state_transition(*status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state);
-		status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
-
-	} else if (link_loss_act == link_loss_actions_t::AUTO_LAND && status_flags.condition_local_position_valid) {
-		status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
-
-	} else if (link_loss_act == link_loss_actions_t::TERMINATE) {
-		status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
-		armed->force_failsafe = true;
-
-	} else if (link_loss_act == link_loss_actions_t::LOCKDOWN) {
-		armed->lockdown = true;
-
-		// do the best you can according to the current state
-
-	} else if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
-		status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
-
-	} else if (status_flags.condition_local_position_valid) {
-		status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
-
-	} else if (status_flags.condition_local_altitude_valid) {
-		if (status->is_rotary_wing) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
-
-		} else {
-			// TODO: FW position controller doesn't run without condition_global_position_valid
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
 		}
 
-	} else {
+	// FALLTHROUGH
+	case (link_loss_actions_t::AUTO_RTL):
+		if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
+			main_state_transition(*status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state);
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+			return;
+		}
+
+	// FALLTHROUGH
+	case (link_loss_actions_t::AUTO_LOITER):
+		if (status_flags.condition_global_position_valid) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+			return;
+		}
+
+	// FALLTHROUGH
+	case (link_loss_actions_t::AUTO_LAND):
+		if (status_flags.condition_global_position_valid) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+			return;
+
+		} else {
+			if (status->is_rotary_wing) {
+				if (status_flags.condition_local_position_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+					return;
+
+				} else if (status_flags.condition_local_altitude_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+					return;
+				}
+
+			} else {
+				// TODO: FW position controller doesn't run without condition_global_position_valid
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+				return;
+			}
+		}
+
+	// FALLTHROUGH
+	case (link_loss_actions_t::TERMINATE):
 		status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+		armed->force_failsafe = true;
+		break;
+
+	case (link_loss_actions_t::LOCKDOWN):
+		armed->lockdown = true;
+		break;
 	}
 }
 

src/modules/commander/state_machine_helper.h

@@ -60,12 +60,12 @@ typedef enum {
 
 enum class link_loss_actions_t {
 	DISABLED = 0,
-	AUTO_LOITER = 1,
+	AUTO_LOITER = 1,	// Hold mode
-	AUTO_RTL = 2,
+	AUTO_RTL = 2,		// Return mode
-	AUTO_LAND = 3,
+	AUTO_LAND = 3,		// Land mode
-	AUTO_RECOVER = 4,
+	AUTO_RECOVER = 4,	// Data Link Auto Recovery (CASA Outback Challenge rules)
-	TERMINATE = 5,
+	TERMINATE = 5,		// Terminate
-	LOCKDOWN = 6,
+	LOCKDOWN = 6,		// Lockdown
 };
 
 typedef enum {

src/modules/mavlink/mavlink_main.cpp

@@ -699,7 +699,7 @@ Mavlink::mavlink_open_uart(int baud, const char *uart_name, bool force_flow_cont
 		/* USB has no baudrate, but use a magic number for 'fast' */
 		_baudrate = 2000000;
 
-		set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB);
+		set_telemetry_status_type(telemetry_status_s::LINK_TYPE_USB);
 	}
 
 #if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
@@ -1580,7 +1580,7 @@ Mavlink::update_rate_mult()
 		// check for RADIO_STATUS timeout and reset
 		if (hrt_elapsed_time(&_rstatus.timestamp) > 5_s) {
 			PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id);
-			set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC);
+			set_telemetry_status_type(telemetry_status_s::LINK_TYPE_GENERIC);
 
 			_radio_status_available = false;
 			_radio_status_critical = false;
@@ -1601,7 +1601,7 @@ void
 Mavlink::update_radio_status(const radio_status_s &radio_status)
 {
 	_rstatus = radio_status;
-	set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO);
+	set_telemetry_status_type(telemetry_status_s::LINK_TYPE_3DR_RADIO);
 
 	/* check hardware limits */
 	_radio_status_available = true;
@@ -2022,7 +2022,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 					} else if (strcmp(myoptarg, "iridium") == 0) {
 						_mode = MAVLINK_MODE_IRIDIUM;
-						set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM);
+						set_telemetry_status_type(telemetry_status_s::LINK_TYPE_IRIDIUM);
 
 					} else if (strcmp(myoptarg, "minimal") == 0) {
 						_mode = MAVLINK_MODE_MINIMAL;
@@ -2233,14 +2233,16 @@ Mavlink::task_main(int argc, char *argv[])
 			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 &&
+				    status.high_latency_data_link_lost &&
 				    !_transmitting_enabled_commanded &&
 				    (_first_heartbeat_sent)) {
+
 					_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) {
+				} else if (!_transmitting_enabled && !status.high_latency_data_link_lost) {
 					_transmitting_enabled = true;
 					mavlink_and_console_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s", _device_name);
 				}
@@ -2558,7 +2560,7 @@ void Mavlink::check_radio_config()
 {
 	/* radio config check */
 	if (_uart_fd >= 0 && _param_radio_id.get() != 0
-	    && _tstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
+	    && _tstatus.type == telemetry_status_s::LINK_TYPE_3DR_RADIO) {
 		/* request to configure radio and radio is present */
 		FILE *fs = fdopen(_uart_fd, "w");
 
@@ -2702,7 +2704,7 @@ Mavlink::display_status()
 		printf("\ttype:\t\t");
 
 		switch (_tstatus.type) {
-		case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
+		case telemetry_status_s::LINK_TYPE_3DR_RADIO:
 			printf("3DR RADIO\n");
 			printf("\t  rssi:\t\t%d\n", _rstatus.rssi);
 			printf("\t  remote rssi:\t%u\n", _rstatus.remote_rssi);
@@ -2713,7 +2715,7 @@ Mavlink::display_status()
 			printf("\t  fixed:\t%u\n", _rstatus.fix);
 			break;
 
-		case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB:
+		case telemetry_status_s::LINK_TYPE_USB:
 			printf("USB CDC\n");
 			break;
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -1882,17 +1882,19 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 		mavlink_heartbeat_t hb;
 		mavlink_msg_heartbeat_decode(msg, &hb);
 
-		/* ignore own heartbeats, accept only heartbeats from GCS */
+		// ignore own heartbeats
-		if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
+		if ((msg->sysid != mavlink_system.sysid) && (msg->compid != mavlink_system.compid)) {
 
 			telemetry_status_s &tstatus = _mavlink->get_telemetry_status();
 
 			/* set heartbeat time and topic time and publish -
 			 * the telem status also gets updated on telemetry events
 			 */
-			tstatus.heartbeat_time = tstatus.timestamp;
+			tstatus.heartbeat_time = hrt_absolute_time();
-			tstatus.system_id = msg->sysid;
+			tstatus.remote_system_id = msg->sysid;
-			tstatus.component_id = msg->compid;
+			tstatus.remote_component_id = msg->compid;
+			tstatus.remote_type = hb.type;
+			tstatus.remote_system_status = hb.system_status;
 		}
 	}
 }

src/modules/navigator/navigator_params.c

@@ -111,48 +111,6 @@ PARAM_DEFINE_FLOAT(NAV_FW_ALTL_RAD, 5.0f);
  */
 PARAM_DEFINE_FLOAT(NAV_MC_ALT_RAD, 0.8f);
 
-/**
- * Set data link loss failsafe mode
- *
- * The data link loss failsafe will only be entered after a timeout,
- * set by COM_DL_LOSS_T in seconds. Once the timeout occurs the selected
- * action will be executed. Setting this parameter to 4 will enable CASA
- * Outback Challenge rules, which are only recommended to participants
- * of that competition.
- *
- * @value 0 Disabled
- * @value 1 Hold mode
- * @value 2 Return mode
- * @value 3 Land mode
- * @value 4 Data Link Auto Recovery (CASA Outback Challenge rules)
- * @value 5 Terminate
- * @value 6 Lockdown
- *
- * @group Mission
- */
-PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
-
-/**
- * Set RC loss failsafe mode
- *
- * The RC loss failsafe will only be entered after a timeout,
- * set by COM_RC_LOSS_T in seconds. If RC input checks have been disabled
- * by setting the COM_RC_IN_MODE param it will not be triggered.
- * Setting this parameter to 4 will enable CASA Outback Challenge rules,
- * which are only recommended to participants of that competition.
- *
- * @value 0 Disabled
- * @value 1 Hold mode
- * @value 2 Return mode
- * @value 3 Land mode
- * @value 4 RC Auto Recovery (CASA Outback Challenge rules)
- * @value 5 Terminate
- * @value 6 Lockdown
- *
- * @group Mission
- */
-PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
-
 /**
  * Set traffic avoidance mode
  *