telemetry_statur: use 4 separate topics

1 files changed, 55 insertions, 32 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 699ced1ab..0003ec106 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -767,7 +767,6 @@ int commander_thread_main(int argc, char *argv[])
 
 	hrt_abstime last_idle_time = 0;
 	hrt_abstime start_time = 0;
-	hrt_abstime latest_heartbeat = 0;
 
 	bool status_changed = true;
 	bool param_init_forced = true;
@@ -797,10 +796,16 @@ int commander_thread_main(int argc, char *argv[])
 	struct offboard_control_setpoint_s sp_offboard;
 	memset(&sp_offboard, 0, sizeof(sp_offboard));
 
-	/* Subscribe to telemetry status */
-	int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
-	struct telemetry_status_s telemetry;
-	memset(&telemetry, 0, sizeof(telemetry));
+	/* Subscribe to telemetry status topics */
+	int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
+	uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM];
+	bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
+
+	for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+		telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
+		telemetry_last_heartbeat[i] = 0;
+		telemetry_lost[i] = true;
+	}
 
 	/* Subscribe to global position */
 	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -882,7 +887,6 @@ int commander_thread_main(int argc, char *argv[])
 	bool arming_state_changed = false;
 	bool main_state_changed = false;
 	bool failsafe_old = false;
-	bool system_checked = false;
 
 	while (!thread_should_exit) {
 
@@ -939,15 +943,6 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
 		}
 
-		/* Perform system checks (again) once params are loaded and MAVLink is up. */
-		if (!system_checked && mavlink_fd && 
-			(telemetry.heartbeat_time > 0) &&
-			(hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) {
-
-			(void)rc_calibration_check(mavlink_fd);
-			system_checked = true;
-		}
-
 		orb_check(sp_man_sub, &updated);
 
 		if (updated) {
@@ -960,10 +955,26 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
 		}
 
-		orb_check(telemetry_sub, &updated);
+		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+			orb_check(telemetry_subs[i], &updated);
 
-		if (updated) {
-			orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry);
+			if (updated) {
+				struct telemetry_status_s telemetry;
+				memset(&telemetry, 0, sizeof(telemetry));
+
+				orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry);
+
+				/* perform system checks when new telemetry link connected */
+				if (mavlink_fd &&
+					telemetry_last_heartbeat[i] == 0 &&
+					telemetry.heartbeat_time > 0 &&
+					hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) {
+
+					(void)rc_calibration_check(mavlink_fd);
+				}
+
+				telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
+			}
 		}
 
 		orb_check(sensor_sub, &updated);
@@ -1367,28 +1378,40 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		/* data link check */
-		if (telemetry.heartbeat_time >= latest_heartbeat) {
-			if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) {
+		/* data links check */
+		bool have_link = false;
+		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+			if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
 				/* handle the case where data link was regained */
-				if (status.data_link_lost) {
-					mavlink_log_critical(mavlink_fd, "#audio: data link regained");
-					status.data_link_lost = false;
-					status_changed = true;
+				if (telemetry_lost[i]) {
+					mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i);
+					telemetry_lost[i] = false;
 				}
-
-				/* Only consider data link with most recent heartbeat */
-				latest_heartbeat = telemetry.heartbeat_time;
+				have_link = true;
 
 			} else {
-				if (!status.data_link_lost) {
-					mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST");
-					status.data_link_lost = true;
-					status_changed = true;
+				if (!telemetry_lost[i]) {
+					mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i);
+					telemetry_lost[i] = 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;
+			}
+
+		} else {
+			if (!status.data_link_lost) {
+				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST");
+				status.data_link_lost = true;
+				status_changed = true;
+			}
+		}
+
 		/* handle commands last, as the system needs to be updated to handle them */
 		orb_check(cmd_sub, &updated);