diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index c820d5a210..d12c51d42f 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -96,7 +96,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = { 4) minimum FBW airspeed (metres/sec) 5) maximum FBW airspeed (metres/sec) */ -int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool stabilize, float aspeed) +int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed) { uint32_t tnow = hal.scheduler->millis(); uint32_t dt = tnow - _last_t; @@ -117,7 +117,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool // Multiply pitch rate error by _ki_rate and integrate // Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs - if (!stabilize && _K_I > 0) { + if (!disable_integrator && _K_I > 0) { float ki_rate = _K_I * _tau; //only integrate if gain and time step are positive and airspeed above min value. if (dt > 0 && aspeed > 0.5f*float(aparm.airspeed_min)) { @@ -230,7 +230,7 @@ float AP_PitchController::get_coordination_rate_offset(void) const // 4) minimum FBW airspeed (metres/sec) // 5) maximum FBW airspeed (metres/sec) // -int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool stabilize) +int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool disable_integrator) { // Calculate offset to pitch rate demand required to maintain pitch angle whilst banking // Calculate ideal turn rate from bank angle and airspeed assuming a level coordinated turn @@ -266,7 +266,7 @@ int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool // Apply the turn correction offset desired_rate = desired_rate + rate_offset; - return _get_rate_out(desired_rate, scaler, stabilize, aspeed); + return _get_rate_out(desired_rate, scaler, disable_integrator, aspeed); } void AP_PitchController::reset_I() diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 1c8cec589b..e3551efb3b 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -19,7 +19,7 @@ public: void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; } int32_t get_rate_out(float desired_rate, float scaler); - int32_t get_servo_out(int32_t angle_err, float scaler, bool stabilize); + int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator); float get_coordination_rate_offset(void) const; void reset_I(); @@ -41,7 +41,7 @@ private: float _integrator; - int32_t _get_rate_out(float desired_rate, float scaler, bool stabilize, float aspeed); + int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed); float _get_coordination_rate_offset(float &aspeed, bool &inverted) const; AP_AHRS *_ahrs; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index b3bad8ceaf..e82d5062cc 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -73,7 +73,7 @@ const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = { internal rate controller, called by attitude and rate controller public functions */ -int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool stabilize) +int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator) { uint32_t tnow = hal.scheduler->millis(); uint32_t dt = tnow - _last_t; @@ -115,7 +115,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool // Multiply roll rate error by _ki_rate and integrate // Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs - if (!stabilize && ki_rate > 0) { + if (!disable_integrator && ki_rate > 0) { //only integrate if gain and time step are positive and airspeed above min value. if (dt > 0 && aspeed > float(aparm.airspeed_min)) { float integrator_delta = rate_error * ki_rate * delta_time; @@ -170,7 +170,7 @@ int32_t AP_RollController::get_rate_out(float desired_rate, float scaler) 3) boolean which is true when stabilise mode is active 4) minimum FBW airspeed (metres/sec) */ -int32_t AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool stabilize) +int32_t AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool disable_integrator) { if (_tau < 0.1) { _tau = 0.1; @@ -179,7 +179,7 @@ int32_t AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool s // Calculate the desired roll rate (deg/sec) from the angle error float desired_rate = angle_err * 0.01f / _tau; - return _get_rate_out(desired_rate, scaler, stabilize); + return _get_rate_out(desired_rate, scaler, disable_integrator); } void AP_RollController::reset_I() diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index a30aa2f3da..e28071cc83 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -18,8 +18,8 @@ public: void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; } - int32_t get_rate_out(float desired_rate, float scaler=1.0); - int32_t get_servo_out(int32_t angle_err, float scaler=1.0, bool stabilize=false); + int32_t get_rate_out(float desired_rate, float scaler); + int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator); void reset_I(); @@ -38,7 +38,7 @@ private: float _integrator; - int32_t _get_rate_out(float desired_rate, float scaler, bool stabilize); + int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator); AP_AHRS *_ahrs; diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index 841d983317..74c81c9599 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/libraries/APM_Control/AP_YawController.cpp @@ -62,7 +62,7 @@ const AP_Param::GroupInfo AP_YawController::var_info[] PROGMEM = { AP_GROUPEND }; -int32_t AP_YawController::get_servo_out(float scaler, bool stabilize) +int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator) { uint32_t tnow = hal.scheduler->millis(); uint32_t dt = tnow - _last_t; @@ -120,7 +120,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stabilize) // Apply integrator, but clamp input to prevent control saturation and freeze integrator below min FBW speed // Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs // Don't integrate if _K_D is zero as integrator will keep winding up - if (!stabilize && _K_D > 0) { + if (!disable_integrator && _K_D > 0) { //only integrate if airspeed above min value if (aspeed > float(aspd_min)) { diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h index ff7b4b22dc..51c556528f 100644 --- a/libraries/APM_Control/AP_YawController.h +++ b/libraries/APM_Control/AP_YawController.h @@ -21,7 +21,7 @@ public: _ins = _ahrs->get_ins(); } - int32_t get_servo_out(float scaler = 1.0, bool stabilize = false); + int32_t get_servo_out(float scaler, bool disable_integrator); void reset_I();