diff --git a/src/modules/interface/pid.h b/src/modules/interface/pid.h index bd8ba837f2..54a94e27de 100644 --- a/src/modules/interface/pid.h +++ b/src/modules/interface/pid.h @@ -104,10 +104,9 @@ typedef struct { - float desired; //< set point + float desired; //< set point float error; //< error float prevError; //< previous error - float errorMax; //< maximum error float integ; //< integral float deriv; //< derivative float kp; //< proportional gain @@ -117,8 +116,7 @@ typedef struct float outI; //< integral output (debugging) float outD; //< derivative output (debugging) float iLimit; //< integral limit - float iLimitLow; //< integral limit - bool iCapped; //< true to stop integration + float outputLimit; //< output 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 b95d73d42d..860923b391 100644 --- a/src/modules/src/attitude_pid_controller.c +++ b/src/modules/src/attitude_pid_controller.c @@ -30,6 +30,7 @@ #include "attitude_controller.h" #include "pid.h" #include "param.h" +#include "log.h" #define ATTITUDE_LPF_CUTOFF_FREQ 15.0f #define ATTITUDE_LPF_ENABLE false @@ -55,9 +56,13 @@ PidObject pidRoll; PidObject pidPitch; PidObject pidYaw; -int16_t rollOutput; -int16_t pitchOutput; -int16_t yawOutput; +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; @@ -89,6 +94,10 @@ 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; } @@ -116,6 +125,10 @@ 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); @@ -151,6 +164,30 @@ void attitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* *yaw = yawOutput; } +LOG_GROUP_START(pid_attitude) +LOG_ADD(LOG_FLOAT, roll_outP, &pidRoll.outP) +LOG_ADD(LOG_FLOAT, roll_outI, &pidRoll.outI) +LOG_ADD(LOG_FLOAT, roll_outD, &pidRoll.outD) +LOG_ADD(LOG_FLOAT, pitch_outP, &pidPitch.outP) +LOG_ADD(LOG_FLOAT, pitch_outI, &pidPitch.outI) +LOG_ADD(LOG_FLOAT, pitch_outD, &pidPitch.outD) +LOG_ADD(LOG_FLOAT, yaw_outP, &pidYaw.outP) +LOG_ADD(LOG_FLOAT, yaw_outI, &pidYaw.outI) +LOG_ADD(LOG_FLOAT, yaw_outD, &pidYaw.outD) +LOG_GROUP_STOP(pid_attitude) + +LOG_GROUP_START(pid_rate) +LOG_ADD(LOG_FLOAT, roll_outP, &pidRollRate.outP) +LOG_ADD(LOG_FLOAT, roll_outI, &pidRollRate.outI) +LOG_ADD(LOG_FLOAT, roll_outD, &pidRollRate.outD) +LOG_ADD(LOG_FLOAT, pitch_outP, &pidPitchRate.outP) +LOG_ADD(LOG_FLOAT, pitch_outI, &pidPitchRate.outI) +LOG_ADD(LOG_FLOAT, pitch_outD, &pidPitchRate.outD) +LOG_ADD(LOG_FLOAT, yaw_outP, &pidYawRate.outP) +LOG_ADD(LOG_FLOAT, yaw_outI, &pidYawRate.outI) +LOG_ADD(LOG_FLOAT, yaw_outD, &pidYawRate.outD) +LOG_GROUP_STOP(pid_rate) + PARAM_GROUP_START(pid_attitude) PARAM_ADD(PARAM_FLOAT, roll_kp, &pidRoll.kp) PARAM_ADD(PARAM_FLOAT, roll_ki, &pidRoll.ki) @@ -164,6 +201,8 @@ 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 0535b502fd..c82a0e0a25 100644 --- a/src/modules/src/pid.c +++ b/src/modules/src/pid.c @@ -34,19 +34,17 @@ void pidInit(PidObject* pid, const float desired, const float kp, const float samplingRate, const float cutoffFreq, bool enableDFilter) { - pid->error = 0; - pid->errorMax = 0.0f; - pid->prevError = 0; - pid->integ = 0; - pid->deriv = 0; - pid->desired = desired; - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->iLimit = DEFAULT_PID_INTEGRATION_LIMIT; - pid->iLimitLow = -DEFAULT_PID_INTEGRATION_LIMIT; - pid->iCapped = false; - pid->dt = dt; + pid->error = 0; + pid->prevError = 0; + pid->integ = 0; + pid->deriv = 0; + pid->desired = desired; + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + pid->iLimit = DEFAULT_PID_INTEGRATION_LIMIT; + pid->outputLimit = DEFAULT_PID_INTEGRATION_LIMIT; + pid->dt = dt; pid->enableDFilter = enableDFilter; if (pid->enableDFilter) { @@ -56,28 +54,15 @@ void pidInit(PidObject* pid, const float desired, const float kp, float pidUpdate(PidObject* pid, const float measured, const bool updateError) { - float output; + float output = 0.0f; if (updateError) { pid->error = pid->desired - measured; - if (pid->errorMax > FLT_MIN) { - float errorMaxScaled = pid->errorMax / pid->kp; - pid->error = constrain(pid->error, -errorMaxScaled, errorMaxScaled); - } } - if (pid->iCapped == false) { - pid->integ += pid->error * pid->dt; - if (pid->integ > pid->iLimit) - { - pid->integ = pid->iLimit; - } - else if (pid->integ < pid->iLimitLow) - { - pid->integ = pid->iLimitLow; - } - } + pid->outP = pid->kp * pid->error; + output += pid->outP; float deriv = (pid->error - pid->prevError) / pid->dt; if (pid->enableDFilter) @@ -86,12 +71,22 @@ float pidUpdate(PidObject* pid, const float measured, const bool updateError) } else { pid->deriv = deriv; } + if (isnan(pid->deriv)) { + pid->deriv = 0; + } + 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->outP = pid->kp * pid->error; pid->outI = pid->ki * pid->integ; - pid->outD = pid->kd * pid->deriv; + output += pid->outI; - output = pid->outP + pid->outI + pid->outD; + output = constrain(output, -pid->outputLimit, pid->outputLimit); pid->prevError = pid->error; @@ -103,10 +98,6 @@ void pidSetIntegralLimit(PidObject* pid, const float limit) { } -void pidSetIntegralLimitLow(PidObject* pid, const float limitLow) { - pid->iLimitLow = limitLow; -} - void pidReset(PidObject* pid) { pid->error = 0; diff --git a/src/modules/src/position_controller_pid.c b/src/modules/src/position_controller_pid.c index c03883dd22..df58ab9281 100644 --- a/src/modules/src/position_controller_pid.c +++ b/src/modules/src/position_controller_pid.c @@ -65,10 +65,12 @@ struct this_s { // Maximum roll/pitch angle permited static float rpLimit = 20; +static float rpLimitOverhead = 1.10f; // Velocity maximums static float xyVelMax = 1.0f; static float zVelMax = 1.0f; static float velMaxOverhead = 1.10f; +static const float thrustScale = 1000.0f; #define DT (float)(1.0f/POSITION_RATE) #define POSITION_LPF_CUTOFF_FREQ 20.0f @@ -96,8 +98,8 @@ static struct this_s this = { .pidVZ = { .init = { - .kp = 25, - .ki = 15, + .kp = 15, + .ki = 20, .kd = 0, }, .pid.dt = DT, @@ -125,7 +127,7 @@ static struct this_s this = { .init = { .kp = 2.0f, .ki = 0, - .kd = 0.01f, + .kd = 0, }, .pid.dt = DT, }, @@ -150,10 +152,6 @@ void positionControllerInit() this.pidVY.pid.dt, POSITION_RATE, POSITION_LPF_CUTOFF_FREQ, POSITION_LPF_ENABLE); pidInit(&this.pidVZ.pid, this.pidVZ.setpoint, this.pidVZ.init.kp, this.pidVZ.init.ki, this.pidVZ.init.kd, this.pidVZ.pid.dt, POSITION_RATE, POSITION_LPF_CUTOFF_FREQ, POSITION_LPF_ENABLE); - - this.pidX.pid.errorMax = xyVelMax * velMaxOverhead; - this.pidY.pid.errorMax = xyVelMax * velMaxOverhead; - this.pidZ.pid.errorMax = zVelMax * velMaxOverhead; } static float runPid(float input, struct pidAxis_s *axis, float setpoint, float dt) { @@ -166,6 +164,12 @@ static float runPid(float input, struct pidAxis_s *axis, float setpoint, float d void positionController(float* thrust, attitude_t *attitude, const state_t *state, const setpoint_t *setpoint) { + this.pidX.pid.outputLimit = xyVelMax * velMaxOverhead; + this.pidY.pid.outputLimit = xyVelMax * velMaxOverhead; + // The ROS landing detector will prematurely trip if + // this value is below 0.5 + this.pidZ.pid.outputLimit = max(zVelMax, 0.5) * velMaxOverhead; + // X, Y pidSetDesired(&this.pidVX.pid, runPid(state->position.x, &this.pidX, setpoint->position.x, DT)); pidSetDesired(&this.pidVY.pid, runPid(state->position.y, &this.pidY, setpoint->position.y, DT)); @@ -177,6 +181,12 @@ void positionController(float* thrust, attitude_t *attitude, const state_t *stat void velocityController(float* thrust, attitude_t *attitude, const state_t *state, const setpoint_t *setpoint) { + this.pidVX.pid.outputLimit = rpLimit * rpLimitOverhead; + this.pidVY.pid.outputLimit = rpLimit * rpLimitOverhead; + // Set the output limit to the maximum thrust range + this.pidVZ.pid.outputLimit = (UINT16_MAX / 2 / thrustScale); + //this.pidVZ.pid.outputLimit = (this.thrustBase - this.thrustMin) / thrustScale; + // Roll and Pitch float rollRaw = pidUpdate(&this.pidVX.pid, state->velocity.x, true); float pitchRaw = pidUpdate(&this.pidVY.pid, state->velocity.y, true); @@ -185,28 +195,16 @@ void velocityController(float* thrust, attitude_t *attitude, const state_t *stat attitude->pitch = -(rollRaw * cosf(yawRad)) - (pitchRaw * sinf(yawRad)); attitude->roll = -(pitchRaw * cosf(yawRad)) + (rollRaw * sinf(yawRad)); - // Check for roll/pitch limits and prevent I from accumulating when capped - if (abs(attitude->roll) > rpLimit || abs(attitude->pitch) > rpLimit) { - this.pidVX.pid.iCapped = true; - this.pidVY.pid.iCapped = true; - } else { - this.pidVX.pid.iCapped = false; - this.pidVY.pid.iCapped = false; - } - attitude->roll = constrain(attitude->roll, -rpLimit, rpLimit); attitude->pitch = constrain(attitude->pitch, -rpLimit, rpLimit); // Thrust float thrustRaw = pidUpdate(&this.pidVZ.pid, state->velocity.z, true); // Scale the thrust and add feed forward term - *thrust = thrustRaw*1000 + this.thrustBase; - // Check for minimum thrust, prevent I from accumulating when capped + *thrust = thrustRaw*thrustScale + this.thrustBase; + // Check for minimum thrust if (*thrust < this.thrustMin) { *thrust = this.thrustMin; - this.pidVZ.pid.iCapped = true; - } else { - this.pidVZ.pid.iCapped = false; } } @@ -281,6 +279,8 @@ PARAM_ADD(PARAM_FLOAT, zKd, &this.pidZ.pid.kd) PARAM_ADD(PARAM_UINT16, thrustBase, &this.thrustBase) PARAM_ADD(PARAM_UINT16, thrustMin, &this.thrustMin) -PARAM_ADD(PARAM_FLOAT, rpLimit, &rpLimit) +PARAM_ADD(PARAM_FLOAT, rpLimit, &rpLimit) +PARAM_ADD(PARAM_FLOAT, xyVelMax, &xyVelMax) +PARAM_ADD(PARAM_FLOAT, zVelMax, &zVelMax) PARAM_GROUP_STOP(posCtlPid) diff --git a/src/utils/interface/num.h b/src/utils/interface/num.h index 033198ed22..cb7b59f34e 100644 --- a/src/utils/interface/num.h +++ b/src/utils/interface/num.h @@ -37,6 +37,9 @@ #undef abs #define abs(a) ((a) > 0 ? (a) : (-1*(a))) +#undef isnan +#define isnan(n) ((n != n) ? 1 : 0) + uint16_t single2half(float number); float half2single(uint16_t number);