constrain integrator part in control output until startup detection is available for safety reasons

src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp

@@ -161,9 +161,10 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 	}
 
 	/* integrator limit */
-	_integrator = math::constrain(_integrator, -_integrator_max, _integrator_max);
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * k_i_rate, -_integrator_max, _integrator_max);
 	/* store non-limited output */
-	_last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_roll_ff)) * scaler;
+	_last_output = ((_rate_error * _k_d * scaler) + integrator_constrained  * scaler + (_rate_setpoint * k_roll_ff)) * scaler;
 
 	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
 }

src/lib/ecl/attitude_fw/ecl_roll_controller.cpp

@@ -133,11 +133,12 @@ float ECL_RollController::control_bodyrate(float pitch,
 	}
 
 	/* integrator limit */
-	_integrator = math::constrain(_integrator, -_integrator_max, _integrator_max);
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * k_i_rate, -_integrator_max, _integrator_max);
 	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
 
 	/* store non-limited output */
-	_last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler;
+	_last_output = ((_rate_error * _k_d * scaler) + integrator_constrained * scaler + (_rate_setpoint * k_ff)) * scaler;
 
 	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
 }

src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp

@@ -147,9 +147,10 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
 	}
 
 	/* integrator limit */
-	_integrator = math::constrain(_integrator, -_integrator_max, _integrator_max);
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * k_i_rate, -_integrator_max, _integrator_max);
 	/* store non-limited output */
-	_last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler;
+	_last_output = ((_rate_error * _k_d * scaler) + integrator_constrained * scaler + (_rate_setpoint * k_ff)) * scaler;
 
 	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
 }

src/modules/fw_att_control/fw_att_control_params.c

@@ -85,7 +85,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
 // @Description		This limits the range in degrees the integrator can wind up to.
 // @Range		0.0 to 45.0
 // @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f);
+PARAM_DEFINE_FLOAT(FW_P_IMAX, 0.2f);
 
 // @DisplayName		Roll feedforward gain.
 // @Description		This compensates during turns and ensures the nose stays level.
@@ -119,7 +119,7 @@ PARAM_DEFINE_FLOAT(FW_R_I, 0.0f);
 // @Description		This limits the range in degrees the integrator can wind up to.
 // @Range		0.0 to 45.0
 // @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f);
+PARAM_DEFINE_FLOAT(FW_R_IMAX, 0.2f);
 
 // @DisplayName		Maximum Roll Rate
 // @Description		This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
@@ -130,7 +130,7 @@ PARAM_DEFINE_FLOAT(FW_R_RMAX, 60);
 
 PARAM_DEFINE_FLOAT(FW_Y_P, 0);
 PARAM_DEFINE_FLOAT(FW_Y_I, 0);
-PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f);
+PARAM_DEFINE_FLOAT(FW_Y_IMAX, 0.2f);
 PARAM_DEFINE_FLOAT(FW_Y_D, 0);
 PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0);
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f);