diff options
4 files changed, 12 insertions, 9 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index eb1031cd3..4b43e73e6 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/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); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index ab3ac0a9d..658a0a501 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/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); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index e56a8d08d..b6938a59a 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/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); } diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index efe40b1ef..39b78e20a 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/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); |