feat(control_allocation): Handle NaN correctly in slew rate

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;
 		}
 	}
 }