diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp
index 07ec2a44e1..4849a7443e 100644
--- a/src/drivers/pwm_out/PWMOut.cpp
+++ b/src/drivers/pwm_out/PWMOut.cpp
@@ -580,9 +580,9 @@ void PWMOut::Run()
 	// push backup schedule
 	ScheduleDelayed(_backup_schedule_interval_us);
 
-	if (_new_mode_request.load() != _mode) {
+	if (_new_mode_request.load() != MODE_NO_REQUEST) {
 		set_mode(_new_mode_request.load());
-		_new_mode_request.store(_mode);
+		_new_mode_request.store(MODE_NO_REQUEST);
 	}
 
 	_mixing_output.update();
@@ -1762,6 +1762,22 @@ int PWMOut::fmu_new_mode(PortMode new_mode)
 	return OK;
 }
 
+void PWMOut::request_mode(Mode new_mode)
+{
+	if (_new_mode_request.load() != MODE_NO_REQUEST) {
+		PX4_ERR("already being set"); // not expected to happen
+		return;
+	}
+
+	_new_mode_request.store(new_mode);
+	ScheduleNow();
+	// wait until processed
+	int max_time = 1000;
+
+	while (_new_mode_request.load() != MODE_NO_REQUEST && max_time-- > 0) {
+		px4_usleep(1000);
+	}
+}
 
 namespace
 {
diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp
index 1fb7a280b4..8215568b9a 100644
--- a/src/drivers/pwm_out/PWMOut.hpp
+++ b/src/drivers/pwm_out/PWMOut.hpp
@@ -118,6 +118,8 @@ public:
 		MODE_4CAP,
 		MODE_5CAP,
 		MODE_6CAP,
+
+		MODE_NO_REQUEST
 	};
 
 	PWMOut() = delete;
@@ -157,7 +159,7 @@ public:
 
 	int		set_mode(Mode mode);
 	Mode		get_mode() { return _mode; }
-	void		request_mode(Mode new_mode) { _new_mode_request.store(new_mode); }
+	void		request_mode(Mode new_mode);
 
 	static int	set_i2c_bus_clock(unsigned bus, unsigned clock_hz);
 
@@ -181,7 +183,7 @@ private:
 
 	Mode		_mode{MODE_NONE};
 
-	px4::atomic<Mode> _new_mode_request{MODE_NONE};
+	px4::atomic<Mode> _new_mode_request{MODE_NO_REQUEST};
 
 	uint32_t	_backup_schedule_interval_us{1_s};