MixingOutput: make mixer loading thread-safe

src/drivers/px4fmu/fmu.cpp

@@ -1461,14 +1461,14 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		}
 
 	case MIXERIOCRESET:
-		_mixing_output.resetMixer();
+		_mixing_output.resetMixerThreadSafe();
 
 		break;
 
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strnlen(buf, 1024);
-			ret = _mixing_output.loadMixer(buf, buflen);
+			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
 			update_pwm_trims();
 
 			break;

src/lib/mixer_module/mixer_module.cpp

@@ -210,6 +210,7 @@ void MixingOutput::unregister()
 bool MixingOutput::update()
 {
 	if (!_mixers) {
+		handleCommands();
 		// do nothing until we have a valid mixer
 		return false;
 	}
@@ -338,6 +339,8 @@ bool MixingOutput::update()
 		}
 	}
 
+	handleCommands();
+
 	return true;
 }
 
@@ -410,24 +413,15 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8
 
 void MixingOutput::resetMixer()
 {
-	// TODO: thread-safe
-
-	lock();
-
 	if (_mixers != nullptr) {
 		delete _mixers;
 		_mixers = nullptr;
 		_groups_required = 0;
 	}
-
-	_interface.ScheduleNow();
-	unlock();
 }
 
 int MixingOutput::loadMixer(const char *buf, unsigned len)
 {
-	// TODO: thread-safe
-
 	if (_mixers == nullptr) {
 		_mixers = new MixerGroup(controlCallback, (uintptr_t)this);
 	}
@@ -451,13 +445,81 @@ int MixingOutput::loadMixer(const char *buf, unsigned len)
 	PX4_DEBUG("loaded mixers \n%s\n", buf);
 
 	updateParams();
+	return ret;
+}
+
+void MixingOutput::handleCommands()
+{
+	if (_command.command.load() == Command::Type::None) {
+		return;
+	}
+
+	switch (_command.command.load()) {
+	case Command::Type::loadMixer:
+		_command.result = loadMixer(_command.mixer_buf, _command.mixer_buf_length);
+		break;
+
+	case Command::Type::resetMixer:
+		resetMixer();
+		_command.result = 0;
+		break;
+
+	default:
+		break;
+	}
+
+	// mark as done
+	_command.command.store(Command::Type::None);
+}
+
+void MixingOutput::resetMixerThreadSafe()
+{
+	if (_command.command.load() != Command::Type::None) {
+		// Cannot happen, because we expect only one other thread to call this.
+		// But as a safety precaution we return here.
+		PX4_ERR("Command not None");
+		return;
+	}
 
 	lock();
 
+	_command.command.store(Command::Type::resetMixer);
+
 	_interface.ScheduleNow();
 
 	unlock();
 
-	return ret;
+	// wait until processed
+	while (_command.command.load() != Command::Type::None) {
+		usleep(1000);
+	}
+
+}
+
+int MixingOutput::loadMixerThreadSafe(const char *buf, unsigned len)
+{
+	if (_command.command.load() != Command::Type::None) {
+		// Cannot happen, because we expect only one other thread to call this.
+		// But as a safety precaution we return here.
+		PX4_ERR("Command not None");
+		return -1;
+	}
+
+	lock();
+
+	_command.mixer_buf = buf;
+	_command.mixer_buf_length = len;
+	_command.command.store(Command::Type::loadMixer);
+
+	_interface.ScheduleNow();
+
+	unlock();
+
+	// wait until processed
+	while (_command.command.load() != Command::Type::None) {
+		usleep(1000);
+	}
+
+	return _command.result;
 }
 

src/lib/mixer_module/mixer_module.hpp

@@ -37,6 +37,7 @@
 #include <lib/mixer/mixer.h>
 #include <lib/perf/perf_counter.h>
 #include <lib/pwm_limit/pwm_limit.h>
+#include <px4_atomic.h>
 #include <px4_module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Publication.hpp>
@@ -112,8 +113,21 @@ public:
 
 	void setMaxTopicUpdateRate(unsigned max_topic_update_interval_us);
 
+	/**
+	 * Reset (unload) the complete mixer, called from another thread.
+	 * This is thread-safe, as long as only one other thread at a time calls this.
+	 */
+	void resetMixerThreadSafe();
+
 	void resetMixer();
 
+	/**
+	 * Load (append) a new mixer from a buffer, called from another thread.
+	 * This is thread-safe, as long as only one other thread at a time calls this.
+	 * @return 0 on success, <0 error otherwise
+	 */
+	int loadMixerThreadSafe(const char *buf, unsigned len);
+
 	int loadMixer(const char *buf, unsigned len);
 
 	const actuator_armed_s &armed() const { return _armed; }
@@ -136,6 +150,8 @@ protected:
 	void updateParams() override;
 
 private:
+	void handleCommands();
+
 	bool armNoThrottle() const
 	{
 		return (_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode;
@@ -147,6 +163,19 @@ private:
 		Betaflight = 1
 	};
 
+	struct Command {
+		enum class Type : int {
+			None,
+			resetMixer,
+			loadMixer
+		};
+		px4::atomic<Type> command{Type::None};
+		const char *mixer_buf;
+		unsigned mixer_buf_length;
+		int result;
+	};
+	Command _command; ///< incoming commands (from another thread)
+
 	/**
 	 * Reorder PWM outputs according to _param_mot_ordering
 	 * @param values PWM values to reorder