Mavlink: Add MAV_x_RADIO_CTL parameter to disable RADIO_STATUS flow control

* Add param for software flow ctl on 3DR radios
* Dont reset telemetry type on radio timeout
* Treat 3DR radio as generic link type
* Rename 3DR to SiK radio

msg/telemetry_status.msg

@@ -1,9 +1,8 @@
 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 LINK_TYPE_UBIQUITY_BULLET = 1
+uint8 LINK_TYPE_WIRE = 2
+uint8 LINK_TYPE_USB = 3
+uint8 LINK_TYPE_IRIDIUM	= 4
 
 # MAV_COMPONENT (fill in as needed)
 uint8 COMPONENT_ID_ALL		= 0

src/modules/mavlink/mavlink_main.cpp

@@ -1530,7 +1530,6 @@ 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::LINK_TYPE_GENERIC);
 
 			_radio_status_available = false;
 			_radio_status_critical = false;
@@ -1551,23 +1550,25 @@ void
 Mavlink::update_radio_status(const radio_status_s &radio_status)
 {
 	_rstatus = radio_status;
-	set_telemetry_status_type(telemetry_status_s::LINK_TYPE_3DR_RADIO);
-
-	/* check hardware limits */
 	_radio_status_available = true;
-	_radio_status_critical = (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE);
 
-	if (radio_status.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
-		/* this indicates link congestion, reduce rate by 20% */
-		_radio_status_mult *= 0.80f;
+	if (_use_software_mav_throttling) {
+
+		/* check hardware limits */
+		_radio_status_critical = (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE);
+
+		if (radio_status.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
+			/* this indicates link congestion, reduce rate by 20% */
+			_radio_status_mult *= 0.80f;
 
-	} else if (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
-		/* this indicates link congestion, reduce rate by 2.5% */
-		_radio_status_mult *= 0.975f;
+		} else if (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
+			/* this indicates link congestion, reduce rate by 2.5% */
+			_radio_status_mult *= 0.975f;
 
-	} else if (radio_status.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
-		/* this indicates spare bandwidth, increase by 2.5% */
-		_radio_status_mult *= 1.025f;
+		} else if (radio_status.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
+			/* this indicates spare bandwidth, increase by 2.5% */
+			_radio_status_mult *= 1.025f;
+		}
 	}
 }
 
@@ -1847,7 +1848,7 @@ Mavlink::task_main(int argc, char *argv[])
 	int temp_int_arg;
 #endif
 
-	while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fwxz", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fswxz", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'b':
 			if (px4_get_parameter_value(myoptarg, _baudrate) != 0) {
@@ -2028,6 +2029,10 @@ Mavlink::task_main(int argc, char *argv[])
 			_forwarding_on = true;
 			break;
 
+		case 's':
+			_use_software_mav_throttling = true;
+			break;
+
 		case 'w':
 			_wait_to_transmit = true;
 			break;
@@ -2253,7 +2258,7 @@ Mavlink::task_main(int argc, char *argv[])
 #endif // CONFIG_NET
 		}
 
-		check_radio_config();
+		configure_sik_radio();
 
 		if (status_sub->update(&status_time, &status)) {
 			/* switch HIL mode if required */
@@ -2603,11 +2608,10 @@ void Mavlink::publish_telemetry_status()
 	_telem_status_pub.publish(_tstatus);
 }
 
-void Mavlink::check_radio_config()
+void Mavlink::configure_sik_radio()
 {
 	/* radio config check */
-	if (_uart_fd >= 0 && _param_mav_radio_id.get() != 0
-	    && _tstatus.type == telemetry_status_s::LINK_TYPE_3DR_RADIO) {
+	if (_uart_fd >= 0 && _param_sik_radio_id.get() != 0) {
 		/* request to configure radio and radio is present */
 		FILE *fs = fdopen(_uart_fd, "w");
 
@@ -2617,9 +2621,9 @@ void Mavlink::check_radio_config()
 			fprintf(fs, "+++\n");
 			px4_usleep(1200000);
 
-			if (_param_mav_radio_id.get() > 0) {
+			if (_param_sik_radio_id.get() > 0) {
 				/* set channel */
-				fprintf(fs, "ATS3=%u\n", _param_mav_radio_id.get());
+				fprintf(fs, "ATS3=%u\n", _param_sik_radio_id.get());
 				px4_usleep(200000);
 
 			} else {
@@ -2650,8 +2654,8 @@ void Mavlink::check_radio_config()
 		}
 
 		/* reset param and save */
-		_param_mav_radio_id.set(0);
-		_param_mav_radio_id.commit_no_notification();
+		_param_sik_radio_id.set(0);
+		_param_sik_radio_id.commit_no_notification();
 	}
 }
 
@@ -2750,9 +2754,8 @@ Mavlink::display_status()
 
 		printf("\ttype:\t\t");
 
-		switch (_tstatus.type) {
-		case telemetry_status_s::LINK_TYPE_3DR_RADIO:
-			printf("3DR RADIO\n");
+		if (_radio_status_available) {
+			printf("RADIO Link\n");
 			printf("\t  rssi:\t\t%d\n", _rstatus.rssi);
 			printf("\t  remote rssi:\t%u\n", _rstatus.remote_rssi);
 			printf("\t  txbuf:\t%u\n", _rstatus.txbuf);
@@ -2760,15 +2763,12 @@ Mavlink::display_status()
 			printf("\t  remote noise:\t%u\n", _rstatus.remote_noise);
 			printf("\t  rx errors:\t%u\n", _rstatus.rxerrors);
 			printf("\t  fixed:\t%u\n", _rstatus.fix);
-			break;
 
-		case telemetry_status_s::LINK_TYPE_USB:
+		} else if (_is_usb_uart) {
 			printf("USB CDC\n");
-			break;
 
-		default:
+		} else {
 			printf("GENERIC LINK OR RADIO\n");
-			break;
 		}
 
 	} else {

src/modules/mavlink/mavlink_main.h

@@ -583,6 +583,7 @@ private:
 
 	bool			_forwarding_on{false};
 	bool			_ftp_on{false};
+	bool			_use_software_mav_throttling{false};
 
 	int			_uart_fd{-1};
 
@@ -662,7 +663,7 @@ private:
 		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
 		(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id,
 		(ParamInt<px4::params::MAV_PROTO_VER>) _param_mav_proto_ver,
-		(ParamInt<px4::params::MAV_RADIO_ID>) _param_mav_radio_id,
+		(ParamInt<px4::params::MAV_SIK_RADIO_ID>) _param_sik_radio_id,
 		(ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
 		(ParamBool<px4::params::MAV_USEHILGPS>) _param_mav_usehilgps,
 		(ParamBool<px4::params::MAV_FWDEXTSP>) _param_mav_fwdextsp,
@@ -725,12 +726,12 @@ private:
 	void check_requested_subscriptions();
 
 	/**
-	 * Check the configuration of a connected radio
+	 * Reconfigure a SiK radio if requested by MAV_SIK_RADIO_ID
 	 *
 	 * This convenience function allows to re-configure a connected
-	 * radio without removing it from the main system harness.
+	 * SiK radio without removing it from the main system harness.
 	 */
-	void check_radio_config();
+	void configure_sik_radio();
 
 	/**
 	 * Update rate mult so total bitrate will be equal to _datarate.

src/modules/mavlink/mavlink_params.c

@@ -59,18 +59,18 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 1);
 PARAM_DEFINE_INT32(MAV_PROTO_VER, 0);
 
 /**
- * MAVLink Radio ID
+ * MAVLink SiK Radio ID
  *
  * When non-zero the MAVLink app will attempt to configure the
- * radio to this ID and re-set the parameter to 0. If the value
+ * SiK radio to this ID and re-set the parameter to 0. If the value
  * is negative it will reset the complete radio config to
- * factory defaults.
+ * factory defaults. Only applies if this mavlink instance is going through a SiK radio
  *
  * @group MAVLink
  * @min -1
  * @max 240
  */
-PARAM_DEFINE_INT32(MAV_RADIO_ID, 0);
+PARAM_DEFINE_INT32(MAV_SIK_RADIO_ID, 0);
 
 /**
  * MAVLink airframe type

src/modules/mavlink/module.yaml

@@ -8,6 +8,10 @@ serial_config:
         then
             set MAV_ARGS "${MAV_ARGS} -f"
         fi
+        if param compare MAV_${i}_RADIO_CTL 1
+        then
+            set MAV_ARGS "${MAV_ARGS} -s"
+        fi
         mavlink start -d ${SERIAL_DEV} ${MAV_ARGS} -x
       port_config_param:
         name: MAV_${i}_CONFIG
@@ -78,3 +82,16 @@ parameters:
             num_instances: *max_num_config_instances
             default: [true, false, false]
 
+        MAV_${i}_RADIO_CTL:
+            description:
+                short: Enable software throttling of mavlink on instance ${i}
+                long: |
+                    If enabled, MAVLink messages will be throttled according to
+                    `txbuf` field reported by radio_status.
+
+                    Requires a radio to send the mavlink message RADIO_STATUS.
+
+            type: boolean
+            reboot_required: true
+            num_instances: *max_num_config_instances
+            default: [true, true, true]