MC rate control - Scale the integrator with K during the integration

part to avoid having to scale its saturation separately. This is
required to avoid premature saturation of the integrator when using
the K term.
Also remove double saturation of the integrator

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -415,9 +415,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt, const Vector3f &rat
 	/* apply low-pass filtering to the rates for D-term */
 	Vector3f rates_filtered(_lp_filters_d.apply(rates));
 
-	_att_control = _rate_k.emult(rates_p_scaled.emult(rates_err) +
-				     _rates_int -
+	_att_control = _rate_k.emult(rates_p_scaled.emult(rates_err) -
 				     rates_d_scaled.emult(rates_filtered - _rates_prev_filtered) / dt) +
+		       _rates_int +
 		       _rate_ff.emult(_rates_sp);
 
 	_rates_prev = rates;
@@ -441,13 +441,11 @@ MulticopterAttitudeControl::control_attitude_rates(float dt, const Vector3f &rat
 			// prevent further positive control saturation
 			if (positive_saturation) {
 				rates_err(i) = math::min(rates_err(i), 0.0f);
-
 			}
 
 			// prevent further negative control saturation
 			if (negative_saturation) {
 				rates_err(i) = math::max(rates_err(i), 0.0f);
-
 			}
 
 			// I term factor: reduce the I gain with increasing rate error.
@@ -460,19 +458,13 @@ MulticopterAttitudeControl::control_attitude_rates(float dt, const Vector3f &rat
 			i_factor = math::max(0.0f, 1.f - i_factor * i_factor);
 
 			// Perform the integration using a first order method and do not propagate the result if out of range or invalid
-			float rate_i = _rates_int(i) + i_factor * rates_i_scaled(i) * rates_err(i) * dt;
-
-			if (PX4_ISFINITE(rate_i) && rate_i > -_rate_int_lim(i) && rate_i < _rate_int_lim(i)) {
-				_rates_int(i) = rate_i;
+			float rate_i = _rates_int(i) + i_factor * _rate_k(i) * rates_i_scaled(i) * rates_err(i) * dt;
 
+			if (PX4_ISFINITE(rate_i)) {
+				_rates_int(i) = math::constrain(rate_i, -_rate_int_lim(i), _rate_int_lim(i));
 			}
 		}
 	}
-
-	/* explicitly limit the integrator state */
-	for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
-		_rates_int(i) = math::constrain(_rates_int(i), -_rate_int_lim(i), _rate_int_lim(i));
-	}
 }
 
 void