diff --git a/src/drivers/pwm_out/CMakeLists.txt b/src/drivers/pwm_out/CMakeLists.txt
index 017865fdb4..d04d39ef74 100644
--- a/src/drivers/pwm_out/CMakeLists.txt
+++ b/src/drivers/pwm_out/CMakeLists.txt
@@ -34,6 +34,8 @@ px4_add_module(
 	MODULE drivers__pwm_out
 	MAIN pwm_out
 	COMPILE_FLAGS
+		-Wno-unused-but-set-variable # TODO
+		-Wno-unused-variable # TODO
 	SRCS
 		PWMOut.cpp
 		PWMOut.hpp
diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp
index 24560ad1e2..b92381c3d3 100644
--- a/src/drivers/pwm_out/PWMOut.cpp
+++ b/src/drivers/pwm_out/PWMOut.cpp
@@ -37,7 +37,16 @@ pthread_mutex_t pwm_out_module_mutex = PTHREAD_MUTEX_INITIALIZER;
 static px4::atomic<PWMOut *> _objects[PWM_OUT_MAX_INSTANCES] {};
 static bool _pwm_out_started = false;
 
-static bool is_running() { return (_objects[0].load() != nullptr) || (_objects[1].load() != nullptr); }
+static bool is_running()
+{
+	for (auto &obj : _objects) {
+		if (obj.load() != nullptr) {
+			return true;
+		}
+	}
+
+	return false;
+}
 
 PWMOut::PWMOut(int instance, uint8_t output_base) :
 	CDev((instance == 0) ? PX4FMU_DEVICE_PATH : PX4FMU_DEVICE_PATH"1"),
@@ -571,6 +580,11 @@ void PWMOut::Run()
 	// push backup schedule
 	ScheduleDelayed(_backup_schedule_interval_us);
 
+	if (_new_mode_request.load() != _mode) {
+		set_mode(_new_mode_request.load());
+		_new_mode_request.store(_mode);
+	}
+
 	_mixing_output.update();
 
 	/* update PWM status if armed or if disarmed PWM values are set */
@@ -1729,18 +1743,24 @@ int PWMOut::fmu_new_mode(PortMode new_mode)
 		return -1;
 	}
 
+#if PWM_OUT_MAX_INSTANCES > 0
 	PWMOut *pwm0 = _objects[0].load(); // TODO: get_instance();
 
 	if (pwm0 && pwm_mode0 != pwm0->get_mode()) {
-		pwm0->set_mode(pwm_mode0);
+		pwm0->request_mode(pwm_mode0);
 	}
 
+#endif
+
+#if PWM_OUT_MAX_INSTANCES > 1
 	PWMOut *pwm1 = _objects[1].load(); // TODO: get_instance();
 
 	if (pwm1 && pwm_mode1 != pwm1->get_mode()) {
-		pwm1->set_mode(pwm_mode1);
+		pwm1->request_mode(pwm_mode1);
 	}
 
+#endif
+
 	return OK;
 }
 
diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp
index c0e56acd92..1fb7a280b4 100644
--- a/src/drivers/pwm_out/PWMOut.hpp
+++ b/src/drivers/pwm_out/PWMOut.hpp
@@ -157,6 +157,7 @@ public:
 
 	int		set_mode(Mode mode);
 	Mode		get_mode() { return _mode; }
+	void		request_mode(Mode new_mode) { _new_mode_request.store(new_mode); }
 
 	static int	set_i2c_bus_clock(unsigned bus, unsigned clock_hz);
 
@@ -180,6 +181,8 @@ private:
 
 	Mode		_mode{MODE_NONE};
 
+	px4::atomic<Mode> _new_mode_request{MODE_NONE};
+
 	uint32_t	_backup_schedule_interval_us{1_s};
 
 	unsigned	_pwm_default_rate{50};
diff --git a/src/systemcmds/pwm/CMakeLists.txt b/src/systemcmds/pwm/CMakeLists.txt
index c7c5e7ea4f..859ce1c8d3 100644
--- a/src/systemcmds/pwm/CMakeLists.txt
+++ b/src/systemcmds/pwm/CMakeLists.txt
@@ -35,7 +35,6 @@ px4_add_module(
 	MAIN pwm
 	COMPILE_FLAGS
 		-Wno-array-bounds
-		-O0
 	SRCS
 		pwm.cpp
 	DEPENDS