From 8ccd40185ac396e00a59e9ce3b9ad07772a38243 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Sat, 25 Jun 2022 09:01:15 +0200
Subject: [PATCH] PWMOut/px4io: use disarmed values as default failsafe values

to avoid surprises where upon disarm an ESC can suddenly spool up
even though it stops when disarmed and no specific failsafe value is configured.
---
 src/drivers/pwm_out/PWMOut.cpp | 52 ++++++++++++++----------------
 src/drivers/px4io/px4io.cpp    | 58 ++++++++++++++++------------------
 2 files changed, 51 insertions(+), 59 deletions(-)

diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp
index 27d1a9ed70..682942a985 100644
--- a/src/drivers/pwm_out/PWMOut.cpp
+++ b/src/drivers/pwm_out/PWMOut.cpp
@@ -600,34 +600,6 @@ void PWMOut::update_params()
 			}
 		}
 
-		// PWM_MAIN_FAILx
-		{
-			sprintf(str, "%s_FAIL%u", prefix, i + 1);
-			int32_t pwm_failsafe = -1;
-
-			if (param_get(param_find(str), &pwm_failsafe) == PX4_OK) {
-				if (pwm_failsafe >= 0) {
-					_mixing_output.failsafeValue(i) = math::constrain(pwm_failsafe, (int32_t) 0, (int32_t) PWM_HIGHEST_MAX);
-
-					if (pwm_failsafe != _mixing_output.failsafeValue(i)) {
-						int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
-						param_set(param_find(str), &pwm_fail_new);
-					}
-
-				} else {
-					if (pwm_default_channel_mask & 1 << i) {
-						_mixing_output.failsafeValue(i) = PWM_MOTOR_OFF;
-
-					} else {
-						_mixing_output.failsafeValue(i) = PWM_SERVO_STOP;
-					}
-				}
-
-			} else {
-				PX4_ERR("param %s not found", str);
-			}
-		}
-
 		// PWM_MAIN_DISx
 		{
 			sprintf(str, "%s_DIS%u", prefix, i + 1);
@@ -655,6 +627,30 @@ void PWMOut::update_params()
 			}
 		}
 
+		// PWM_MAIN_FAILx
+		{
+			sprintf(str, "%s_FAIL%u", prefix, i + 1);
+			int32_t pwm_failsafe = -1;
+
+			if (param_get(param_find(str), &pwm_failsafe) == PX4_OK) {
+				if (pwm_failsafe >= 0) {
+					_mixing_output.failsafeValue(i) = math::constrain(pwm_failsafe, (int32_t) 0, (int32_t) PWM_HIGHEST_MAX);
+
+					if (pwm_failsafe != _mixing_output.failsafeValue(i)) {
+						int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
+						param_set(param_find(str), &pwm_fail_new);
+					}
+
+				} else {
+					// if no channel specific failsafe value is configured, use the disarmed value
+					_mixing_output.failsafeValue(i) = _mixing_output.disarmedValue(i);
+				}
+
+			} else {
+				PX4_ERR("param %s not found", str);
+			}
+		}
+
 		// PWM_MAIN_REVx
 		{
 			sprintf(str, "%s_REV%u", prefix, i + 1);
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index f16f81c256..5c08d53381 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -788,37 +788,6 @@ void PX4IO::update_params()
 		_pwm_max_configured = true;
 	}
 
-	// PWM_MAIN_FAILx
-	if (!_pwm_fail_configured) {
-		for (unsigned i = 0; i < _max_actuators; i++) {
-			sprintf(str, "%s_FAIL%u", prefix, i + 1);
-			int32_t pwm_fail = -1;
-
-			if (param_get(param_find(str), &pwm_fail) == PX4_OK) {
-				if (pwm_fail >= 0) {
-					_mixing_output.failsafeValue(i) = math::constrain(pwm_fail, static_cast<int32_t>(0),
-									  static_cast<int32_t>(PWM_HIGHEST_MAX));
-
-					if (pwm_fail != _mixing_output.failsafeValue(i)) {
-						int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
-						param_set(param_find(str), &pwm_fail_new);
-					}
-
-				} else {
-					if (pwm_default_channel_mask & 1 << i) {
-						_mixing_output.failsafeValue(i) = PWM_MOTOR_OFF;
-
-					} else {
-						_mixing_output.failsafeValue(i) = PWM_SERVO_STOP;
-					}
-				}
-			}
-		}
-
-		_pwm_fail_configured = true;
-		updateFailsafe();
-	}
-
 	// PWM_MAIN_DISx
 	if (!_pwm_dis_configured) {
 		for (unsigned i = 0; i < _max_actuators; i++) {
@@ -845,6 +814,33 @@ void PX4IO::update_params()
 		updateDisarmed();
 	}
 
+	// PWM_MAIN_FAILx
+	if (!_pwm_fail_configured) {
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			sprintf(str, "%s_FAIL%u", prefix, i + 1);
+			int32_t pwm_fail = -1;
+
+			if (param_get(param_find(str), &pwm_fail) == PX4_OK) {
+				if (pwm_fail >= 0) {
+					_mixing_output.failsafeValue(i) = math::constrain(pwm_fail, static_cast<int32_t>(0),
+									  static_cast<int32_t>(PWM_HIGHEST_MAX));
+
+					if (pwm_fail != _mixing_output.failsafeValue(i)) {
+						int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
+						param_set(param_find(str), &pwm_fail_new);
+					}
+
+				} else {
+					// if no channel specific failsafe value is configured, use the disarmed value
+					_mixing_output.failsafeValue(i) = _mixing_output.disarmedValue(i);
+				}
+			}
+		}
+
+		_pwm_fail_configured = true;
+		updateFailsafe();
+	}
+
 	// PWM_MAIN_REVx
 	if (!_pwm_rev_configured) {
 		uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();