fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 15:14:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

PWMOut/px4io: use disarmed values as default failsafe values

Browse Source 
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.
This commit is contained in:
Matthias Grob 2022-06-25 09:01:15 +02:00
parent 85a5dd87cd
commit 8ccd40185a
2 changed files with 51 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
src/drivers/pwm_out/PWMOut.cpp
View File

@ -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);

58
src/drivers/px4io/px4io.cpp
View File

@ -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();