diff --git a/src/modules/interface/pid.h b/src/modules/interface/pid.h index 54a94e27de..51005cdd12 100644 --- a/src/modules/interface/pid.h +++ b/src/modules/interface/pid.h @@ -63,7 +63,8 @@ #define PID_YAW_INTEGRATION_LIMIT 360.0 -#define DEFAULT_PID_INTEGRATION_LIMIT 5000.0 +#define DEFAULT_PID_INTEGRATION_LIMIT 5000.0 +#define DEFAULT_PID_OUTPUT_LIMIT 0.0 #else @@ -98,7 +99,8 @@ #define PID_YAW_INTEGRATION_LIMIT 360.0 -#define DEFAULT_PID_INTEGRATION_LIMIT 10000.0 +#define DEFAULT_PID_INTEGRATION_LIMIT 10000.0 +#define DEFAULT_PID_OUTPUT_LIMIT 0.0 #endif @@ -115,8 +117,8 @@ typedef struct float outP; //< proportional output (debugging) float outI; //< integral output (debugging) float outD; //< derivative output (debugging) - float iLimit; //< integral limit - float outputLimit; //< output limit + float iLimit; //< integral limit, absolute value. '0' means no limit. + float outputLimit; //< total PID output limit, absolute value. '0' means no limit. float dt; //< delta-time dt lpf2pData dFilter; //< filter for D term bool enableDFilter; //< filter for D term enable flag diff --git a/src/modules/src/attitude_pid_controller.c b/src/modules/src/attitude_pid_controller.c index 4fd68d605e..4f9bf5872b 100644 --- a/src/modules/src/attitude_pid_controller.c +++ b/src/modules/src/attitude_pid_controller.c @@ -60,10 +60,6 @@ static int16_t rollOutput; static int16_t pitchOutput; static int16_t yawOutput; -static float rpRateLimit = 150.0f; -static float yawRateLimit = 150.0f; -static float rpyRateLimitOverhead = 1.1f; - static bool isInit; void attitudeControllerInit(const float updateDt) @@ -94,10 +90,6 @@ void attitudeControllerInit(const float updateDt) pidSetIntegralLimit(&pidPitch, PID_PITCH_INTEGRATION_LIMIT); pidSetIntegralLimit(&pidYaw, PID_YAW_INTEGRATION_LIMIT); - pidRollRate.outputLimit = INT16_MAX; - pidPitchRate.outputLimit = INT16_MAX; - pidYawRate.outputLimit = INT16_MAX; - isInit = true; } @@ -125,10 +117,6 @@ void attitudeControllerCorrectAttitudePID( float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired, float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired) { - pidRoll.outputLimit = rpRateLimit * rpyRateLimitOverhead; - pidPitch.outputLimit = rpRateLimit * rpyRateLimitOverhead; - pidYaw.outputLimit = yawRateLimit * rpyRateLimitOverhead; - pidSetDesired(&pidRoll, eulerRollDesired); *rollRateDesired = pidUpdate(&pidRoll, eulerRollActual, true); @@ -201,8 +189,6 @@ PARAM_ADD(PARAM_FLOAT, yaw_kd, &pidYaw.kd) PARAM_GROUP_STOP(pid_attitude) PARAM_GROUP_START(pid_rate) -PARAM_ADD(PARAM_FLOAT, rpRateLimit, &rpRateLimit) -PARAM_ADD(PARAM_FLOAT, yawRateLimit, &yawRateLimit) PARAM_ADD(PARAM_FLOAT, roll_kp, &pidRollRate.kp) PARAM_ADD(PARAM_FLOAT, roll_ki, &pidRollRate.ki) PARAM_ADD(PARAM_FLOAT, roll_kd, &pidRollRate.kd) diff --git a/src/modules/src/pid.c b/src/modules/src/pid.c index 4f4b2e765c..c756d4c0b4 100644 --- a/src/modules/src/pid.c +++ b/src/modules/src/pid.c @@ -43,7 +43,7 @@ void pidInit(PidObject* pid, const float desired, const float kp, pid->ki = ki; pid->kd = kd; pid->iLimit = DEFAULT_PID_INTEGRATION_LIMIT; - pid->outputLimit = DEFAULT_PID_INTEGRATION_LIMIT; + pid->outputLimit = DEFAULT_PID_OUTPUT_LIMIT; pid->dt = dt; pid->enableDFilter = enableDFilter; if (pid->enableDFilter) @@ -77,16 +77,23 @@ float pidUpdate(PidObject* pid, const float measured, const bool updateError) pid->outD = pid->kd * pid->deriv; output += pid->outD; - float i = pid->integ + pid->error * pid->dt; - // Check if integral is saturated - if (abs(i) <= pid->iLimit || abs(pid->ki * i + output) <= pid->outputLimit) { - pid->integ = i; + pid->integ += pid->error * pid->dt; + + // Constrain the integral (unless the iLimit is zero) + if(pid->iLimit != 0) + { + pid->integ = constrain(pid->integ, -pid->iLimit, pid->iLimit); } pid->outI = pid->ki * pid->integ; output += pid->outI; - output = constrain(output, -pid->outputLimit, pid->outputLimit); + // Constrain the total PID output (unless the outputLimit is zero) + if(pid->outputLimit != 0) + { + output = constrain(output, -pid->outputLimit, pid->outputLimit); + } + pid->prevError = pid->error;