diff --git a/src/lib/rate_control/rate_control.hpp b/src/lib/rate_control/rate_control.hpp index 1db18ee092..b2469bc77b 100644 --- a/src/lib/rate_control/rate_control.hpp +++ b/src/lib/rate_control/rate_control.hpp @@ -94,6 +94,18 @@ public: */ void resetIntegral() { _rate_int.zero(); } + /** + * Set the integral term to 0 to prevent windup for each axis + * @param axis axis: roll 0 / pitch 1 / yaw 2 + * @see _rate_int + */ + void resetIntegral(size_t axis) + { + if (axis < 3) { + _rate_int(axis) = 0.0f; + } + } + /** * Get status message of controller for logging/debugging * @param rate_ctrl_status status message to fill with internal states diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 970d3c77e7..9ff446b1f3 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -118,6 +118,10 @@ FixedwingRateControl::vehicle_manual_poll() _rate_sp_pub.publish(_rates_sp); + if (_param_fw_acro_yaw_int_reset.get()) { + _rate_control.resetIntegral(2); + } + } else { /* manual/direct control */ diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index d24edd0d84..48576ae6b0 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -152,6 +152,7 @@ private: (ParamFloat) _param_fw_acro_x_max, (ParamFloat) _param_fw_acro_y_max, (ParamFloat) _param_fw_acro_z_max, + (ParamInt) _param_fw_acro_yaw_int_reset, (ParamFloat) _param_fw_airspd_max, (ParamFloat) _param_fw_airspd_min, diff --git a/src/modules/fw_rate_control/fw_rate_control_params.c b/src/modules/fw_rate_control/fw_rate_control_params.c index 10d8172bdf..58f53a328e 100644 --- a/src/modules/fw_rate_control/fw_rate_control_params.c +++ b/src/modules/fw_rate_control/fw_rate_control_params.c @@ -365,6 +365,16 @@ PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90); */ PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45); +/** + * Reset for integral of the z axis while rate controller in acro mode. + * + * This configures the integral of each axis of the rate controller in acro mode + * + * @boolean + * @group FW Rate Control + */ +PARAM_DEFINE_INT32(FW_ACRO_Z_IRESET, 1); + /** * Enable throttle scale by battery level *