mavlink: move thread handling into MavlinkReceiver

In my opinion this makes it much cleaner and will allow mavlink main to
directly call the receiver.

src/modules/mavlink/mavlink_main.cpp

@@ -94,7 +94,8 @@ hrt_abstime Mavlink::_first_start_time = {0};
 bool Mavlink::_boot_complete = false;
 
 Mavlink::Mavlink() :
-	ModuleParams(nullptr)
+	ModuleParams(nullptr),
+	_receiver(this)
 {
 	// initialise parameter cache
 	mavlink_update_parameters();
@@ -2229,8 +2230,7 @@ Mavlink::task_main(int argc, char *argv[])
 		send_autopilot_capabilities();
 	}
 
-	/* start the MAVLink receiver last to avoid a race */
-	MavlinkReceiver::receive_start(&_receive_thread, this);
+	_receiver.start();
 
 	_mavlink_start_time = hrt_absolute_time();
 
@@ -2513,8 +2513,7 @@ Mavlink::task_main(int argc, char *argv[])
 		perf_end(_loop_perf);
 	}
 
-	/* first wait for threads to complete before tearing down anything */
-	pthread_join(_receive_thread, nullptr);
+	_receiver.stop();
 
 	delete _subscribe_to_stream;
 	_subscribe_to_stream = nullptr;

src/modules/mavlink/mavlink_main.h

@@ -80,6 +80,7 @@
 
 #include "mavlink_command_sender.h"
 #include "mavlink_messages.h"
+#include "mavlink_receiver.h"
 #include "mavlink_shell.h"
 #include "mavlink_ulog.h"
 
@@ -527,6 +528,7 @@ public:
 	bool radio_status_critical() const { return _radio_status_critical; }
 
 private:
+	MavlinkReceiver 	_receiver;
 	int			_instance_id{0};
 
 	bool			_transmitting_enabled{true};
@@ -573,8 +575,6 @@ private:
 
 	mavlink_channel_t	_channel{MAVLINK_COMM_0};
 
-	pthread_t		_receive_thread {};
-
 	bool			_forwarding_on{false};
 	bool			_ftp_on{false};
 	bool			_use_software_mav_throttling{false};

src/modules/mavlink/mavlink_receiver.cpp

@@ -2887,11 +2887,8 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
 	_gimbal_device_information_pub.publish(gimbal_information);
 }
 
-/**
- * Receive data from UART/UDP
- */
 void
-MavlinkReceiver::Run()
+MavlinkReceiver::run()
 {
 	/* set thread name */
 	{
@@ -3154,17 +3151,7 @@ void MavlinkReceiver::update_rx_stats(const mavlink_message_t &message)
 	}
 }
 
-void *
-MavlinkReceiver::start_helper(void *context)
-{
-	MavlinkReceiver rcv{(Mavlink *)context};
-	rcv.Run();
-
-	return nullptr;
-}
-
-void
-MavlinkReceiver::receive_start(pthread_t *thread, Mavlink *parent)
+void MavlinkReceiver::start()
 {
 	pthread_attr_t receiveloop_attr;
 	pthread_attr_init(&receiveloop_attr);
@@ -3177,7 +3164,20 @@ MavlinkReceiver::receive_start(pthread_t *thread, Mavlink *parent)
 	pthread_attr_setstacksize(&receiveloop_attr,
 				  PX4_STACK_ADJUSTED(sizeof(MavlinkReceiver) + 2840 + MAVLINK_RECEIVER_NET_ADDED_STACK));
 
-	pthread_create(thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
+	pthread_create(_thread, &receiveloop_attr, MavlinkReceiver::start_trampoline, this);
 
 	pthread_attr_destroy(&receiveloop_attr);
 }
+
+void *MavlinkReceiver::start_trampoline(void *context)
+{
+	MavlinkReceiver *self = reinterpret_cast<MavlinkReceiver *>(context);
+	self->run();
+	return nullptr;
+}
+
+void MavlinkReceiver::stop()
+{
+	_should_exit.store(true);
+	pthread_join(*_thread, nullptr);
+}

src/modules/mavlink/mavlink_receiver.h

@@ -121,14 +121,12 @@ public:
 	MavlinkReceiver(Mavlink *parent);
 	~MavlinkReceiver() override;
 
-	/**
-	 * Start the receiver thread
-	 */
-	static void receive_start(pthread_t *thread, Mavlink *parent);
-
-	static void *start_helper(void *context);
+	void start();
+	void stop();
 
 private:
+	static void *start_trampoline(void *context);
+	void run();
 
 	void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result);
 
@@ -199,8 +197,6 @@ private:
 
 	void CheckHeartbeats(const hrt_abstime &t, bool force = false);
 
-	void Run();
-
 	/**
 	 * Set the interval at which the given message stream is published.
 	 * The rate is the number of messages per second.
@@ -227,6 +223,9 @@ private:
 
 	void update_rx_stats(const mavlink_message_t &message);
 
+	px4::atomic_bool 	_should_exit{false};
+	pthread_t		*_thread {};
+
 	Mavlink				*_mavlink;
 
 	MavlinkFTP			_mavlink_ftp;