diff --git a/src/lib/control_allocation/control_allocation/ControlAllocation.cpp b/src/lib/control_allocation/control_allocation/ControlAllocation.cpp index dfdf9ca7f7..7278108a8c 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocation.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocation.cpp @@ -110,17 +110,54 @@ const void ControlAllocation::applySlewRateLimit(float dt) { + // For motors, an actuator setpoint of NaN represents switching off the + // motor (giving it disarmed PWM). Physically it results in zero thrust, + // therefore for the purpose of slew limiting we need to consider NaN + // equivalent to zero. But after the slew limiting, we again replace by + // NaN. + + // We want the slew rate to behave like this on different input transitions: + // - between 0 and NaN: immediately match input + // - nonzero to NaN: sink to zero with slew rate, then replace zero by NaN + // - NaN to nonzero: replace NaN by zero, then rise with slew rate to input + // - between nonzero and 0: slew limit, then match input + for (int i = 0; i < _num_actuators; i++) { if (_actuator_slew_rate_limit(i) > FLT_EPSILON) { - float delta_sp_max = dt * (_actuator_max(i) - _actuator_min(i)) / _actuator_slew_rate_limit(i); - float delta_sp = _actuator_sp(i) - _prev_actuator_sp(i); + + float input = _actuator_sp(i); + float previous = _prev_actuator_sp(i); + + // Before slew limiting, transform NaN to 0, but remember if the input was NaN + const bool input_is_nan = !PX4_ISFINITE(input); + + if (input_is_nan) { + input = 0.f; + } + + if (!PX4_ISFINITE(previous)) { + previous = 0.f; + } + + // Slew limit without any NaN involved + const float delta_sp_max = dt * (_actuator_max(i) - _actuator_min(i)) / _actuator_slew_rate_limit(i); + const float delta_sp = input - previous; + + float output = input; if (delta_sp > delta_sp_max) { - _actuator_sp(i) = _prev_actuator_sp(i) + delta_sp_max; + output = previous + delta_sp_max; } else if (delta_sp < -delta_sp_max) { - _actuator_sp(i) = _prev_actuator_sp(i) - delta_sp_max; + output = previous - delta_sp_max; } + + // Transform back to NaN if appropriate + if (input_is_nan && fabsf(output) < FLT_EPSILON) { + output = NAN; + } + + _actuator_sp(i) = output; } } }