manual merge fw_integrator_fix_ouputlimit into fw_staging_ouputlimit

src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp

@@ -160,10 +160,11 @@ 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, -_integrator_max, _integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	_last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
 //	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
 //	warnx("roll: _last_output %.4f", (double)_last_output);
 	return math::constrain(_last_output, -1.0f, 1.0f);

src/lib/ecl/attitude_fw/ecl_roll_controller.cpp

@@ -132,12 +132,12 @@ float ECL_RollController::control_bodyrate(float pitch,
 	}
 
 	/* integrator limit */
-	_integrator = math::constrain(_integrator, -_integrator_max, _integrator_max);
-//	warnx("roll: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i);
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
-//	warnx("roll: _last_output %.4f", (double)_last_output);
+	_last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
 
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }

src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp

@@ -146,10 +146,11 @@ 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, -_integrator_max, _integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	_last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
 	//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
 
 

src/modules/fw_att_control/fw_att_control_params.c

@@ -85,7 +85,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.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_PR_IMAX, 1000.0f);
+PARAM_DEFINE_FLOAT(FW_PR_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_RR_I, 0.05f);
 // @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_RR_IMAX, 1000.0f);
+PARAM_DEFINE_FLOAT(FW_RR_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_YR_P, 0.5);
 PARAM_DEFINE_FLOAT(FW_YR_I, 0.05);
-PARAM_DEFINE_FLOAT(FW_YR_IMAX, 1000.0f);
+PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
 PARAM_DEFINE_FLOAT(FW_YR_D, 0); //xxx: remove
 PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0);
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f);