DP-8528 mavlink orb poll

boards/px4/fmu-v5x/ssrc.px4board

@@ -19,6 +19,10 @@ CONFIG_SYSTEMCMDS_NETMAN=n
 CONFIG_SYSTEMCMDS_REFLECT=n
 CONFIG_SYSTEMCMDS_SD_BENCH=n
 CONFIG_SYSTEMCMDS_SD_STRESS=n
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
+CONFIG_SYSTEMCMDS_WORK_QUEUE=n
+CONFIG_SYSTEMCMDS_SD_BENCH=n
+CONFIG_SYSTEMCMDS_SD_STRESS=n
 
 # Test keys to help local building
 # These can be overridden in CI with environment variables pointing to real keys

platforms/common/uORB/Subscription.hpp

@@ -143,6 +143,17 @@ class Subscription
 		return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, true) : false;
 	}
 
+	void ack()
+	{
+		if (!valid()) {
+			subscribe();
+		}
+
+		if (valid()) {
+			Manager::orb_data_ack(_node, _last_generation);
+		}
+	}
+
 	/**
 	 * Copy the struct
 	 * @param dst The uORB message struct we are updating.

platforms/common/uORB/SubscriptionInterval.hpp

@@ -100,6 +100,17 @@ class SubscriptionInterval
 		return false;
 	}
 
+	/**
+	 * Acknowledge a new update.
+	 */
+	void ack()
+	{
+		_subscription.ack();
+		const hrt_abstime now = hrt_absolute_time();
+		// shift last update time forward, but don't let it get further behind than the interval
+		_last_update = math::constrain(_last_update + _interval_us, now - _interval_us, now);
+	}
+
 	/**
 	 * Copy the struct if updated.
 	 * @param dst The destination pointer where the struct will be copied.

platforms/common/uORB/uORB.cpp

@@ -130,6 +130,11 @@ int orb_check(orb_sub_t handle, bool *updated)
 	return uORB::Manager::get_instance()->orb_check(handle, updated);
 }
 
+int orb_ack(orb_sub_t handle)
+{
+	return uORB::Manager::get_instance()->orb_ack(handle);
+}
+
 int orb_exists(const struct orb_metadata *meta, int instance)
 {
 	return uORB::Manager::orb_exists(meta, instance);

platforms/common/uORB/uORB.h

@@ -246,6 +246,8 @@ extern int	orb_copy(const struct orb_metadata *meta, orb_sub_t handle, void *buf
  */
 extern int	orb_check(orb_sub_t handle, bool *updated) __EXPORT;
 
+extern int	orb_ack(orb_sub_t handle) __EXPORT;
+
 /**
  * @see uORB::Manager::orb_exists()
  */

platforms/common/uORB/uORBDeviceNode.hpp

@@ -219,6 +219,8 @@ class DeviceNode
 	 */
 	bool copy(void *dst, orb_advert_t &handle, unsigned &generation);
 
+	void ack(unsigned &generation) { generation = _generation.load(); }
+
 	static bool register_callback(orb_advert_t &node_handle, SubscriptionCallback *callback_sub, int8_t poll_lock,
 				      hrt_abstime last_update, uint32_t interval_us, uorb_cb_handle_t &cb_handle)
 	{

platforms/common/uORB/uORBManager.cpp

@@ -365,6 +365,12 @@ int uORB::Manager::orb_check(orb_sub_t handle, bool *updated)
 	return PX4_OK;
 }
 
+int uORB::Manager::orb_ack(orb_sub_t handle)
+{
+	((uORB::SubscriptionInterval *)handle)->ack();
+	return PX4_OK;
+}
+
 int uORB::Manager::orb_set_interval(orb_sub_t handle, unsigned interval)
 {
 	((uORB::SubscriptionInterval *)handle)->set_interval_us(interval * 1000);

platforms/common/uORB/uORBManager.hpp

@@ -291,6 +291,9 @@ class Manager
 	 */
 	static int  orb_check(orb_sub_t handle, bool *updated);
 
+
+	static int  orb_ack(orb_sub_t handle);
+
 	/**
 	 * Check if a topic has already been created and published (advertised)
 	 *
@@ -356,6 +359,11 @@ class Manager
 
 	static uint8_t orb_get_queue_size(const orb_advert_t &node_handle) {return node(node_handle)->get_queue_size();}
 
+	static void orb_data_ack(orb_advert_t &node_handle, unsigned &generation)
+	{
+		node(node_handle)->ack(generation);
+	}
+
 	static bool orb_data_copy(orb_advert_t &node_handle, void *dst, unsigned &generation,
 				  bool only_if_updated)
 	{

src/modules/mavlink/Kconfig

@@ -24,3 +24,11 @@ menuconfig USER_MAVLINK
 	depends on BOARD_PROTECTED && MODULES_MAVLINK
 	---help---
 		Put mavlink in userspace memory
+
+menuconfig MAVLINK_UORB_POLL
+	bool "mavlink uorb poll in main loop"
+	default n
+	depends on MODULES_MAVLINK
+	---help---
+		Use uorb topic polling in mavlink main loop instead of
+		using px4_usleep for stream rate handling.

src/modules/mavlink/mavlink_main.cpp

@@ -134,6 +134,10 @@ Mavlink::Mavlink() :
 
 	_event_sub.subscribe();
 	_telemetry_status_pub.advertise();
+
+#if defined(CONFIG_MAVLINK_UORB_POLL)
+	_stream_poller = new MavlinkStreamPoll();
+#endif
 }
 
 Mavlink::~Mavlink()
@@ -175,6 +179,14 @@ Mavlink::~Mavlink()
 		}
 	}
 
+#if defined(CONFIG_MAVLINK_UORB_POLL)
+
+	if (_stream_poller) {
+		delete _stream_poller;
+	}
+
+#endif
+
 	perf_free(_loop_perf);
 	perf_free(_loop_interval_perf);
 	perf_free(_send_byte_error_perf);
@@ -1156,7 +1168,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 		if (strcmp(stream_name, stream->get_name()) == 0) {
 			if (interval != 0) {
 				/* set new interval */
-				stream->set_interval(interval);
+				set_stream_interval(stream, interval);
 
 			} else {
 				/* delete stream */
@@ -1173,7 +1185,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 	MavlinkStream *stream = create_mavlink_stream(stream_name, this);
 
 	if (stream != nullptr) {
-		stream->set_interval(interval);
+		set_stream_interval(stream, interval);
 		_streams.add(stream);
 
 		return OK;
@@ -2325,9 +2337,29 @@ Mavlink::task_main(int argc, char *argv[])
 
 	_task_running.store(true);
 
+#if defined(CONFIG_MAVLINK_UORB_POLL)
+	int uorb_poll_error_counter = 0;
+#endif
+
 	while (!should_exit()) {
 		/* main loop */
+#if defined(CONFIG_MAVLINK_UORB_POLL)
+		int uorb_poll_ret = _stream_poller->poll(MAIN_LOOP_DELAY);
+
+		if (uorb_poll_ret < 0) {
+			/* this is seriously bad - should be an emergency */
+			if (uorb_poll_error_counter < 10 || uorb_poll_error_counter % 50 == 0) {
+				/* use a counter to prevent flooding (and slowing us down) */
+				PX4_ERR("ERROR while polling uorbs: %d", uorb_poll_ret);
+			}
+
+			uorb_poll_error_counter++;
+		}
+
+		_stream_poller->ack_all();
+#else
 		px4_usleep(_main_loop_delay);
+#endif
 
 		if (!should_transmit()) {
 			check_requested_subscriptions();
@@ -2849,6 +2881,43 @@ void Mavlink::configure_sik_radio()
 	}
 }
 
+
+int
+Mavlink::register_orb_poll(uint16_t stream_id, ORB_ID *orbs, int count)
+{
+#if defined(CONFIG_MAVLINK_UORB_POLL)
+	return _stream_poller->register_orbs(stream_id, orbs, count);
+#else
+	(void)stream_id;
+	(void)orbs;
+	(void)count;
+	return PX4_OK;
+#endif
+}
+
+int
+Mavlink::unregister_orb_poll(uint16_t stream_id)
+{
+#if defined(CONFIG_MAVLINK_UORB_POLL)
+	return _stream_poller->unregister_orbs(stream_id);
+#else
+	(void)stream_id;
+	return PX4_OK;
+#endif
+}
+
+int
+Mavlink::set_stream_interval(MavlinkStream *stream, int interval)
+{
+	stream->set_interval(interval);
+#if defined(CONFIG_MAVLINK_UORB_POLL)
+	return _stream_poller->set_interval(stream->get_id(), interval / 1000);
+#else
+	return PX4_OK;
+#endif
+}
+
+
 int Mavlink::start_helper(int argc, char *argv[])
 {
 	/* create the instance in task context */

src/modules/mavlink/mavlink_main.h

@@ -524,7 +524,19 @@ class Mavlink final : public ModuleParams
 
 	bool radio_status_critical() const { return _radio_status_critical; }
 
+	/**
+	 * Register/Unregister a stream orbs for polling
+	 */
+	int register_orb_poll(uint16_t stream_id, ORB_ID *orbs, int count);
+	int unregister_orb_poll(uint16_t stream_id);
+
+	int set_stream_interval(MavlinkStream *stream, int interval);
+
 private:
+#if defined(CONFIG_MAVLINK_UORB_POLL)
+	MavlinkStreamPoll	*_stream_poller {nullptr};
+#endif
+
 	MavlinkReceiver 	_receiver;
 
 	int			_instance_id{-1};