rusefi
/
ChibiOS-Contrib
mirror of https://github.com/rusefi/ChibiOS-Contrib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Cleaning PID lib.

This commit is contained in:
Fabien Poussin 2018-03-22 16:58:48 +01:00
parent d66ce4b75b
commit 12552897f3
2 changed files with 44 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
os/various/pid.c
View File

@ -16,22 +16,23 @@
* reliable defaults, so we need to have the user set them.
***************************************************************************/
void pid_create(pid_t* p, float* Input, float* Output, float* Setpoint,
float Kp, float Ki, float Kd, int POn, int ControllerDirection)
float Kp, float Ki, float Kd, int POn, int Direction)
{
p->myOutput = Output;
p->myInput = Input;
p->mySetpoint = Setpoint;
p->output = Output;
p->input = Input;
p->setPoint = Setpoint;
p->inAuto = false;
pid_setOutputLimits(p, 0, 255); // default output limit corresponds to
// the arduino pwm limits
p->SampleTime = 100; // default Controller Sample Time is 100ms
p->sampleTime = 100; // default Controller Sample Time is 100ms
pid_setControllerDirection(p, ControllerDirection);
pid_setDirection(p, Direction);
pid_setTunings(p, Kp, Ki, Kd, POn);
pid_initialize(p);
p->lastTime = TIME_MS - p->SampleTime;
p->lastTime = TIME_MS - p->sampleTime;
}
@ -46,11 +47,11 @@ bool pid_compute(pid_t* p)
if(!p->inAuto) return false;
unsigned long now = TIME_MS;
unsigned long timeChange = (now - p->lastTime);
if(timeChange >= p->SampleTime)
if(timeChange >= p->sampleTime)
{
/* Compute all the working error variables */
float input = *p->myInput;
float error = *p->mySetpoint - input;
float input = *p->input;
float error = *p->setPoint - input;
float dInput = (input - p->lastInput);
p->outputSum += (p->ki * error);
@ -70,7 +71,7 @@ bool pid_compute(pid_t* p)
if(output > p->outMax) output = p->outMax;
else if(output < p->outMin) output = p->outMin;
*p->myOutput = output;
*p->output = output;
/* Remember some variables for next time */
p->lastInput = input;
@ -87,19 +88,21 @@ bool pid_compute(pid_t* p)
******************************************************************************/
void pid_setTunings(pid_t* p, float Kp, float Ki, float Kd, int POn)
{
if (Kp<0 || Ki<0 || Kd<0) return;
if (Kp < 0 || Ki < 0 || Kd < 0) return;
p->pOn = POn;
p->pOnE = POn == PID_P_ON_E;
p->dispKp = Kp; p->dispKi = Ki; p->dispKd = Kd;
p->dispKp = Kp;
p->dispKi = Ki;
p->dispKd = Kd;
float SampleTimeInSec = ((float)p->SampleTime)/1000;
float SampleTimeInSec = ((float)p->sampleTime) / 1000.0;
p->kp = Kp;
p->ki = Ki * SampleTimeInSec;
p->kd = Kd / SampleTimeInSec;
if(p->controllerDirection == PID_REVERSE)
if(p->direction == PID_REVERSE)
{
p->kp = (0 - p->kp);
p->ki = (0 - p->ki);
@ -114,10 +117,10 @@ void pid_setSampleTime(pid_t* p, int NewSampleTime)
{
if (NewSampleTime > 0)
{
float ratio = (float)NewSampleTime / (float)p->SampleTime;
float ratio = (float)NewSampleTime / (float)p->sampleTime;
p->ki *= ratio;
p->kd /= ratio;
p->SampleTime = (unsigned long)NewSampleTime;
p->sampleTime = (unsigned long)NewSampleTime;
}
}
@ -137,8 +140,8 @@ void pid_setOutputLimits(pid_t* p, float Min, float Max)
if(p->inAuto)
{
if(*p->myOutput > p->outMax) *p->myOutput = p->outMax;
else if(*p->myOutput < p->outMin) *p->myOutput = p->outMin;
if(*p->output > p->outMax) *p->output = p->outMax;
else if(*p->output < p->outMin) *p->output = p->outMin;
if(p->outputSum > p->outMax) p->outputSum = p->outMax;
else if(p->outputSum < p->outMin) p->outputSum = p->outMin;
@ -166,8 +169,8 @@ void pid_setMode(pid_t* p, int Mode)
******************************************************************************/
void pid_initialize(pid_t* p)
{
p->outputSum = *p->myOutput;
p->lastInput = *p->myInput;
p->outputSum = *p->output;
p->lastInput = *p->input;
if(p->outputSum > p->outMax) p->outputSum = p->outMax;
else if(p->outputSum < p->outMin) p->outputSum = p->outMin;
}
@ -178,14 +181,14 @@ void pid_initialize(pid_t* p)
* know which one, because otherwise we may increase the output when we should
* be decreasing. This is called from the constructor.
******************************************************************************/
void pid_setControllerDirection(pid_t* p, int Direction)
void pid_setDirection(pid_t* p, int Direction)
{
if(p->inAuto && Direction != p->controllerDirection)
if(p->inAuto && Direction != p->direction)
{
p->kp = (0 - p->kp);
p->ki = (0 - p->ki);
p->kd = (0 - p->kd);
}
p->controllerDirection = Direction;
p->direction = Direction;
}

34
os/various/pid.h
View File

@ -14,26 +14,26 @@
typedef struct {
float kp; // * (P)roportional Tuning Parameter
float ki; // * (I)ntegral Tuning Parameter
float kd; // * (D)erivative Tuning Parameter
float dispKp; // * we'll hold on to the tuning parameters in user-entered
float dispKi; // format for display purposes
float dispKd; //
float kp; // * (P)roportional Tuning Parameter
float ki; // * (I)ntegral Tuning Parameter
float kd; // * (D)erivative Tuning Parameter
int controllerDirection;
int direction;
int pOn;
float *myInput; // * Pointers to the Input, Output, and Setpoint variables
float *myOutput; // This creates a hard link between the variables and the
float *mySetpoint; // PID, freeing the user from having to constantly tell us
// what these values are. with pointers we'll just know.
float *input; // * Pointers to the Input, Output, and Setpoint variables
float *output; // This creates a hard link between the variables and the
float *setPoint; // PID, freeing the user from having to constantly tell us
// what these values are. with pointers we'll just know.
unsigned long lastTime;
float outputSum;
float lastInput;
unsigned long SampleTime;
unsigned long sampleTime;
float outMin;
float outMax;
@ -45,15 +45,15 @@ typedef struct {
//commonly used functions **************************************************************************
void pid_create(pid_t* p, float* Input, float* Output, float* Setpoint, // * constructor. links the PID to the Input, Output, and
float Kp, float Ki, float Kd, int POn, int ControllerDirection); // Setpoint. Initial tuning parameters are also set here.
// (overload for specifying proportional mode)
float Kp, float Ki, float Kd, int POn, int Direction); // Setpoint. Initial tuning parameters are also set here.
// (overload for specifying proportional mode)
void pid_setmode(pid_t* p, int mode); // * sets PID to either Manual (0) or Auto (non-0)
bool pid_compute(pid_t* p); // * performs the PID calculation. it should be
// called every time loop() cycles. ON/OFF and
// calculation frequency can be set using SetMode
// SetSampleTime respectively
// SetsampleTime respectively
void pid_setOutputLimits(pid_t* p, float Min, float Max); // * clamps the output to a specific range. 0-255 by default, but
// it's likely the user will want to change this depending on
@ -66,10 +66,10 @@ void pid_setTunings(pid_t* p, float Kp, float Ki, float Kd, int POn); // * Whil
// constructor, this function gives the user the option
// of changing tunings during runtime for Adaptive control
void pid_setControllerDirection(pid_t* p, int Direction); // * Sets the Direction, or "Action" of the controller. DIRECT
// means the output will increase when error is positive. REVERSE
// means the opposite. it's very unlikely that this will be needed
// once it is set in the constructor.
void pid_setDirection(pid_t* p, int Direction); // * Sets the Direction, or "Action" of the controller. DIRECT
// means the output will increase when error is positive. REVERSE
// means the opposite. it's very unlikely that this will be needed
// once it is set in the constructor.
void pid_setSampleTime(pid_t* p, int NewSampleTime); // * sets the frequency, in Milliseconds, with which
// the PID calculation is performed. default is 100