mavlink: reduce rate multiplier to 0.25 while sending parameters

Co-authored-by: alexcekay <alexander@auterion.com>

Author: bkueng

src/modules/mavlink/mavlink_main.cpp

@@ -1318,6 +1318,11 @@ Mavlink::update_rate_mult()
 	/* scale up and down as the link permits */
 	float bandwidth_mult = (float)(_datarate * mavlink_ulog_streaming_rate_inv - const_rate) / rate;
 
+	/* Reduce rate while sending parameters in low bandwidth mode */
+	if (sending_parameters() && _mode == Mavlink::MAVLINK_MODE_LOW_BANDWIDTH) {
+		bandwidth_mult = fminf(bandwidth_mult, 0.25f);
+	}
+
 	/* if we do not have flow control, limit to the set data rate */
 	if (!get_flow_control_enabled()) {
 		bandwidth_mult = fminf(1.0f, bandwidth_mult);

src/modules/mavlink/mavlink_main.h

@@ -180,6 +180,9 @@ class Mavlink final : public ModuleParams
 	void			check_events_enable() { _should_check_events.store(true); }
 	void			check_events_disable() { _should_check_events.store(false); }
 
+	bool			sending_parameters() const { return _sending_parameters.load(); }
+	void			set_sending_parameters(bool sending) { _sending_parameters.store(sending); }
+
 	int			get_uart_fd() const { return _uart_fd; }
 
 	/**
@@ -570,6 +573,7 @@ class Mavlink final : public ModuleParams
 	bool			_received_messages{false};	/**< Whether we've received valid mavlink messages. */
 
 	px4::atomic_bool	_should_check_events{false};    /**< Events subscription: only one MAVLink instance should check */
+	px4::atomic_bool	_sending_parameters{false};     /**< True if parameters are currently sent out */
 
 	unsigned		_main_loop_delay{1000};	/**< mainloop delay, depends on data rate */
 

src/modules/mavlink/mavlink_parameters.cpp

@@ -552,6 +552,8 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
 		_mavlink_resend_uart(_mavlink.get_channel(), &mavlink_packet);
 	}
 
+	_last_param_sent = hrt_absolute_time();
+
 	return 0;
 }
 

src/modules/mavlink/mavlink_parameters.h

@@ -77,6 +77,17 @@ class MavlinkParametersManager
 
 	void handle_message(const mavlink_message_t *msg);
 
+	/**
+	 * Check if parameters are sent out. This includes:
+	 * - while sending all parameters
+	 * - while sending a single requested parameter
+	 * - while sending out changed parameters
+	 */
+	bool send_active() const
+	{
+		return hrt_absolute_time() < _last_param_sent + 2_s;
+	}
+
 private:
 	int		_send_all_index{-1};
 
@@ -161,6 +172,8 @@ class MavlinkParametersManager
 
 	Mavlink &_mavlink;
 
+	hrt_abstime _last_param_sent{0};
+
 	bool _first_send{false};
 	hrt_abstime _last_param_sent_timestamp{0}; // time at which the last parameter was sent
 };

src/modules/mavlink/mavlink_receiver.cpp

@@ -3291,6 +3291,7 @@ MavlinkReceiver::run()
 
 			if (_mavlink.get_mode() != Mavlink::MAVLINK_MODE::MAVLINK_MODE_IRIDIUM) {
 				_parameters_manager.send();
+				_mavlink.set_sending_parameters(_parameters_manager.send_active());
 			}
 
 			if (_mavlink.ftp_enabled()) {