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(0), - static_cast(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(0), + static_cast(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();