Adopt high latency switching to new MAV_CMD design

msg/vehicle_command.msg

@@ -72,7 +72,7 @@ uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002	# Control a pre-programmed pay
 uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started
 uint16 VEHICLE_CMD_LOGGING_START = 2510		# start streaming ULog data
 uint16 VEHICLE_CMD_LOGGING_STOP = 2511			# stop streaming ULog data
-uint16 VEHICLE_CMD_MAVLINK_ENABLE_SENDING = 2600	# Start/Stop transmitting data from all instances in mavlink from a certain type
+uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600	# control starting/stopping transmitting data over the high latency link
 
 uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0			# Command ACCEPTED and EXECUTED |
 uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1	# Command TEMPORARY REJECTED/DENIED |

src/modules/commander/commander.cpp

@@ -84,10 +84,6 @@
 #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>
 #include <uORB/topics/battery_status.h>
@@ -146,11 +142,6 @@ 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) */
 
@@ -1315,9 +1306,7 @@ 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 */
@@ -2607,7 +2596,6 @@ 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
@@ -2616,9 +2604,8 @@ Commander::run()
 				mavlink_log_critical(&mavlink_log_pub, "LOW LATENCY DATA LINKS REGAINED, DEACTIVATING 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;
-				vehicle_cmd.param2 = MAVLINK_DISABLE_TRANSMITTING;
+				vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY;
+				vehicle_cmd.param1 = 0.0f;
 				if (vehicle_cmd_pub != nullptr) {
 					orb_publish(ORB_ID(vehicle_command), vehicle_cmd_pub, &vehicle_cmd);
 
@@ -2626,18 +2613,14 @@ Commander::run()
 					vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd);
 				}
 			}
-#endif
-
 		} else {
-#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
 				status.high_latency_data_link_active = true;
 				status_changed = true;
 				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;
-				vehicle_cmd.param2 = MAVLINK_ENABLE_TRANSMITTING;
+				vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY;
+				vehicle_cmd.param1 = 1.0f;
 				if (vehicle_cmd_pub != nullptr) {
 					orb_publish(ORB_ID(vehicle_command), vehicle_cmd_pub, &vehicle_cmd);
 
@@ -2651,9 +2634,6 @@ Commander::run()
 					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");
 				}

src/modules/mavlink/mavlink_main.cpp

@@ -990,6 +990,7 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 		return;
 	}
 
+
 	_last_write_try_time = hrt_absolute_time();
 
 	if (_mavlink_start_time == 0) {
@@ -1036,7 +1037,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 		_last_write_success_time = _last_write_try_time;
 		count_txbytes(packet_len);
 	}
-
 }
 
 void
@@ -2200,18 +2200,14 @@ Mavlink::task_main(int argc, char *argv[])
 		struct vehicle_command_s vehicle_cmd;
 
 		if (cmd_sub->update(&cmd_time, &vehicle_cmd)) {
-			if (vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_MAVLINK_ENABLE_SENDING) {
-				if (_mode == (int)roundf(vehicle_cmd.param1)) {
-					if (_transmitting_enabled != (int)vehicle_cmd.param2) {
-						if ((int)vehicle_cmd.param2) {
-							PX4_INFO("Enable transmitting with mavlink instance of type %s on device %s", mavlink_mode_str(_mode), _device_name);
-
-						} else {
-							PX4_INFO("Disable transmitting with mavlink instance of type %s on device %s", mavlink_mode_str(_mode), _device_name);
-						}
-
-						_transmitting_enabled = (int)vehicle_cmd.param2;
-					}
+			if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) && (_mode == MAVLINK_MODE_IRIDIUM)) {
+				if (!_transmitting_enabled && (vehicle_cmd.param1 > 0.5f)) {
+					PX4_INFO("Enable transmitting with mavlink instance of type %s on device %s", mavlink_mode_str(_mode), _device_name);
+					_transmitting_enabled = true;
+
+				} else if (_transmitting_enabled && (vehicle_cmd.param1 <= 0.5f)) {
+					PX4_INFO("Disable transmitting with mavlink instance of type %s on device %s", mavlink_mode_str(_mode), _device_name);
+					_transmitting_enabled = false;
 				}
 			}
 		}

src/modules/mavlink/mavlink_messages.cpp

@@ -4202,7 +4202,7 @@ protected:
 
 			// failure flags, requires an initial value of 0, set by the default values
 			if (_status_flags_time > 0) {
-				if (status_flags.gps_failure || !status_flags.condition_global_position_valid) {
+				if (!status_flags.condition_global_position_valid) { //TODO check if there is a better way to get only GPS failure
 					msg.failure_flags |= HL_FAILURE_FLAG_GPS;
 				}