From 3289e0cee607f227d6e667034273bb230a73d9fb Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 25 Jul 2016 11:27:34 +0200
Subject: [PATCH] fmu: don't disarm with disarmed set

If disarmed PWM values have been set, we can't accept a disarm command.
If we did, the PWM output stop alltogether after the high pulse of the
ESC calibration.
---
 src/drivers/px4fmu/fmu.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index f78962b01f..61d671c83c 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1533,7 +1533,12 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_DISARM:
-		update_pwm_out_state(false);
+
+		/* Ignore disarm if disarmed PWM is set already. */
+		if (_num_disarmed_set == 0) {
+			update_pwm_out_state(false);
+		}
+
 		break;
 
 	case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: