commander: Fix for qurt and improve high latency switch logic

src/modules/commander/commander.cpp

@@ -84,7 +84,9 @@
 #include <cfloat>
 #include <cstring>
 
+#ifndef __PX4_QURT
 #include <mavlink/mavlink_main.h>
+#endif
 
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
@@ -144,8 +146,10 @@ typedef enum VEHICLE_MODE_FLAG
 
 #define INAIR_RESTART_HOLDOFF_INTERVAL	500000
 
+#ifndef __PX4_QURT
 #define MAVLINK_ENABLE_TRANSMITTING	1	/**< parameter value to enable transmitting for a mavlink instance */
 #define MAVLINK_DISABLE_TRANSMITTING	0	/**< parameter value to disable transmitting for a mavlink instance */
+#endif
 
 /* Controls the probation period which is the amount of time required for position and velocity checks to pass before the validity can be changed from false to true*/
 #define POSVEL_PROBATION_TAKEOFF 30		/**< probation duration set at takeoff (sec) */
@@ -1311,7 +1315,9 @@ Commander::run()
 	/* command ack */
 	orb_advert_t command_ack_pub = nullptr;
 	orb_advert_t commander_state_pub = nullptr;
+#ifndef __PX4_QURT
 	orb_advert_t vehicle_cmd_pub = nullptr;
+#endif
 	orb_advert_t vehicle_status_flags_pub = nullptr;
 
 	/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
@@ -2601,6 +2607,7 @@ Commander::run()
 				status_changed = true;
 			}
 
+#ifndef __PX4_QURT
 			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
@@ -2619,20 +2626,14 @@ Commander::run()
 					vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd);
 				}
 			}
+#endif
 
 		} else {
-			if (!status.data_link_lost && high_latency_link_exists && !status.high_latency_data_link_active && armed.armed &&
-					internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL &&
-					internal_state.main_state != commander_state_s::MAIN_STATE_ACRO &&
-					internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE &&
-					internal_state.main_state != commander_state_s::MAIN_STATE_STAB &&
-					internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL &&
-					internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL ) {
+#ifndef __PX4_QURT
+			if (high_latency_link_exists && !status.high_latency_data_link_active && armed.armed) {
 				// low latency telemetry lost and high latency link existing
-				// only go to the high latency status if armed and not in a manual mode to avoid unnecessary transmitting data
 				status.high_latency_data_link_active = true;
 				status_changed = true;
-				mavlink_log_critical(&mavlink_log_pub, "ALL LOW LATENCY DATA LINKS LOST, ACTIVATING HIGH LATENCY LINK");
 				vehicle_command_s vehicle_cmd;
 				vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_MAVLINK_ENABLE_SENDING;
 				vehicle_cmd.param1 = Mavlink::MAVLINK_MODE::MAVLINK_MODE_IRIDIUM;
@@ -2643,7 +2644,16 @@ Commander::run()
 				} 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");
+				} else {
+					mavlink_log_critical(&mavlink_log_pub, "ACTIVATING AVAILABLE HIGH LATENCY LINK");
+				}
 			} else if (!status.data_link_lost) {
+#else
+			if (!status.data_link_lost) {
+#endif
 				if (armed.armed) {
 					mavlink_log_critical(&mavlink_log_pub, "ALL DATA LINKS LOST");
 				}