diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 917078a824..0527059757 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c588619da7..f997ab2ece 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/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}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9c57817834..fc59ccb5c6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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(context); + self->run(); + return nullptr; +} + +void MavlinkReceiver::stop() +{ + _should_exit.store(true); + pthread_join(*_thread, nullptr); +} diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 020d36c882..47868c2238 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/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;