Skip to content

Commit

Permalink
Merge branch 'PSVL-pid_update'
Browse files Browse the repository at this point in the history
  • Loading branch information
@ataffanel
ataffanel committed Nov 24, 2016
2 parents ba6fa4d + c012b67 commit d614849
Show file tree
Hide file tree
Showing 5 changed files with 96 additions and 65 deletions.
6 changes: 2 additions & 4 deletions src/modules/interface/pid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
45 changes: 42 additions & 3 deletions src/modules/src/attitude_pid_controller.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;

Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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)
Expand All @@ -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)
Expand Down
63 changes: 27 additions & 36 deletions src/modules/src/pid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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)
Expand All @@ -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;

Expand All @@ -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;
Expand Down
44 changes: 22 additions & 22 deletions src/modules/src/position_controller_pid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -96,8 +98,8 @@ static struct this_s this = {

.pidVZ = {
.init = {
.kp = 25,
.ki = 15,
.kp = 15,
.ki = 20,
.kd = 0,
},
.pid.dt = DT,
Expand Down Expand Up @@ -125,7 +127,7 @@ static struct this_s this = {
.init = {
.kp = 2.0f,
.ki = 0,
.kd = 0.01f,
.kd = 0,
},
.pid.dt = DT,
},
Expand All @@ -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) {
Expand All @@ -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));
Expand All @@ -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);
Expand All @@ -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;
}
}

Expand Down Expand Up @@ -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)
3 changes: 3 additions & 0 deletions src/utils/interface/num.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down

0 comments on commit d614849

Please sign in to comment.