pid and low pass filter
Added pid controller functions and low pass filter functions
This commit is contained in:
johan9107
parent
d3112e7128
commit
f0c400552d
37
UAV-ControlSystem/inc/Flight/filter.h
Normal file
37
UAV-ControlSystem/inc/Flight/filter.h
Normal file
@ -0,0 +1,37 @@
|
||||
/**************************************************************************
|
||||
* NAME: filter.h *
|
||||
* AUTHOR: Johan Gärtner *
|
||||
*
|
||||
* PURPOSE: This file contains filter functions *
|
||||
* INFORMATION: *
|
||||
* GLOBAL VARIABLES: *
|
||||
* Variable Type Description *
|
||||
* -------- ---- ----------- *
|
||||
*
|
||||
* **************************************************************************/
|
||||
|
||||
|
||||
|
||||
#ifndef FLIGHT_FILTER_H_
|
||||
#define FLIGHT_FILTER_H_
|
||||
|
||||
|
||||
#define M_PIf 3.14159265358979323846f /*Float number of pi*/
|
||||
|
||||
#include<stdio.h>
|
||||
#include<stdint.h>
|
||||
|
||||
/*Struct of inputs to filter*/
|
||||
typedef struct pt1Filter_s {
|
||||
float state;
|
||||
float RC;
|
||||
float dT;
|
||||
} pt1Filter_t;
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: First order lowpass filter *
|
||||
* INFORMATION: *
|
||||
**************************************************************************/
|
||||
float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT);
|
||||
|
||||
#endif /* FLIGHT_FILTER_H_ */
|
||||
97
UAV-ControlSystem/inc/Flight/pid.h
Normal file
97
UAV-ControlSystem/inc/Flight/pid.h
Normal file
@ -0,0 +1,97 @@
|
||||
/**************************************************************************
|
||||
* NAME: pid.h *
|
||||
* AUTHOR: Johan Gärtner *
|
||||
*
|
||||
* PURPOSE: This file contains pid functions *
|
||||
* INFORMATION: pidUAV is the final pid controller which only takes *
|
||||
* pidProfile_t as input. pidProfile_t will be updated and *
|
||||
* contains all the information about the current state of *
|
||||
* the pid. *
|
||||
* GLOBAL VARIABLES: *
|
||||
* Variable Type Description *
|
||||
* -------- ---- ----------- *
|
||||
*
|
||||
* **************************************************************************/
|
||||
#ifndef FLIGHT_PID_H_
|
||||
#define FLIGHT_PID_H_
|
||||
|
||||
#include<stdio.h>
|
||||
#include<stdint.h>
|
||||
|
||||
#include "Flight/filter.h"
|
||||
|
||||
#define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/
|
||||
#define ITERM_SCALE 0.244381f /*I-term used as a scale value to the PID controller*/
|
||||
#define DTERM_SCALE 0.000529f /*D-term used as a scale value to the PID controller*/
|
||||
|
||||
#define XYZ_AXIS_COUNT 3 /*The maximum number of DOF that belongings to the PID*/
|
||||
|
||||
#define THROTTLE 0 /*Index terms to the PID*/
|
||||
#define ROLL 0 /*Index terms to the PID*/
|
||||
#define PITCH 1 /*Index terms to the PID*/
|
||||
#define YAW 2 /*Index terms to the PID*/
|
||||
|
||||
/*Struct that belongs to a certain PID controller*/
|
||||
typedef struct pidProfile_s {
|
||||
|
||||
bool pidStabilisationEnabled; /*Enables/Dissables PID controller*/
|
||||
|
||||
uint8_t ID_profile; /*ID of a certain PID, shall be referenced to a certain sensor*/
|
||||
uint8_t DOF; /*DOF = degrees of freedom*/
|
||||
|
||||
uint8_t P[XYZ_AXIS_COUNT]; /*PID value*/
|
||||
uint8_t I[XYZ_AXIS_COUNT]; /*PID value*/
|
||||
uint8_t D[XYZ_AXIS_COUNT]; /*PID value*/
|
||||
|
||||
int16_t Max_PID_Term; /*The max and minimum output value of the PID*/
|
||||
int16_t Max_Error_Term; /*The max and minimum output value of the PID error*/
|
||||
|
||||
int16_t Max_P_Term; /*The max and minimum output value of the PTerm*/
|
||||
int16_t Max_I_Term; /*The max and minimum output value of the ITerm*/
|
||||
int16_t Max_D_Term; /*The max and minimum output value of the DTerm*/
|
||||
|
||||
uint16_t dterm_lpf; /*Derivative low pass filter in hz, 80 - 90 is a good init value (same as clean flight)*/
|
||||
uint16_t yaw_lpf; /*Proportional (p term) low pass filter for yaw axis in hz, 80 - 90 is a good init value (same as clean flight)*/
|
||||
|
||||
pt1Filter_t deltaFilter[XYZ_AXIS_COUNT];/*Struct, Contains filter value for each PID of D term, do not to be init*/
|
||||
pt1Filter_t yawFilter; /*Struct, Contains filter value for each PID of p term for yaw axis, do not to be init*/
|
||||
|
||||
float Integral[XYZ_AXIS_COUNT]; /*Buffer which contains integral values, do not to be init */
|
||||
float LastError[XYZ_AXIS_COUNT]; /*Buffer which contains last error values, do not to be init */
|
||||
float PID_Out[XYZ_AXIS_COUNT]; /*Output values of PID loop*/
|
||||
float dT; /*Delta time of each PID cycle*/
|
||||
|
||||
} pidProfile_t;
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Constrain float values within a defined limit *
|
||||
* INFORMATION: Used in PID loop to limit values *
|
||||
**************************************************************************/
|
||||
float constrainf(float amt, int low, int high);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Update current sensor values *
|
||||
* INFORMATION: *
|
||||
**************************************************************************/
|
||||
void getCurrentValues(float *sensorValues, uint8_t ID_profile);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Update desired values from rc command *
|
||||
* INFORMATION: *
|
||||
**************************************************************************/
|
||||
void getPointRate(float *desiredCommand, uint8_t ID_profile);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Initializes the pid profile PID controller *
|
||||
* INFORMATION: Recommended to use if unexpected values occur of profile *
|
||||
**************************************************************************/
|
||||
void pidUAVInit(pidProfile_t *pidProfile);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Dynamic PID controller, able to handle several PID controller *
|
||||
* connected to different profiles.
|
||||
* INFORMATION: *
|
||||
**************************************************************************/
|
||||
void pidUAV(pidProfile_t *pidProfile);
|
||||
|
||||
#endif /* FLIGHT_PID_H_ */
|
||||
31
UAV-ControlSystem/src/Flight/filter.c
Normal file
31
UAV-ControlSystem/src/Flight/filter.c
Normal file
@ -0,0 +1,31 @@
|
||||
/**************************************************************************
|
||||
* NAME: filter.c *
|
||||
* AUTHOR: Johan Gärtner *
|
||||
*
|
||||
* PURPOSE: This file contains filter functions *
|
||||
* INFORMATION: *
|
||||
* GLOBAL VARIABLES: *
|
||||
* Variable Type Description *
|
||||
* -------- ---- ----------- *
|
||||
*
|
||||
* **************************************************************************/
|
||||
|
||||
#include "Flight/filter.h"
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: First order lowpass filter *
|
||||
* INFORMATION: *
|
||||
**************************************************************************/
|
||||
float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
|
||||
{
|
||||
/*Pre calculate and store RC*/
|
||||
if (!filter->RC) {
|
||||
filter->RC = 1.0f / (2.0f * M_PIf * f_cut);
|
||||
filter->dT = dT;
|
||||
}
|
||||
|
||||
/*Adds the new input to the old value of the low pass filter*/
|
||||
filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
|
||||
|
||||
return filter->state;
|
||||
}
|
||||
251
UAV-ControlSystem/src/Flight/pid.c
Normal file
251
UAV-ControlSystem/src/Flight/pid.c
Normal file
@ -0,0 +1,251 @@
|
||||
/**************************************************************************
|
||||
* NAME: pid.c *
|
||||
* AUTHOR: Johan Gärtner *
|
||||
*
|
||||
* PURPOSE: This file contains pid functions *
|
||||
* INFORMATION: pidUAV is the final pid controller which only takes *
|
||||
* pidProfile_t as input. pidProfile_t will be updated and *
|
||||
* contains all the information about the current state of *
|
||||
* the pid. *
|
||||
* GLOBAL VARIABLES: *
|
||||
* Variable Type Description *
|
||||
* -------- ---- ----------- *
|
||||
*
|
||||
* **************************************************************************/
|
||||
|
||||
#include "Flight/pid.h"
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Constrain float values within a defined limit *
|
||||
* INFORMATION: Used in PID loop to limit values *
|
||||
**************************************************************************/
|
||||
float constrainf(float amt, int low, int high)
|
||||
{
|
||||
if (amt < low)
|
||||
return low;
|
||||
else if (amt > high)
|
||||
return high;
|
||||
else
|
||||
return amt;
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Update current sensor values *
|
||||
* INFORMATION: *
|
||||
**************************************************************************/
|
||||
void getCurrentValues(float *sensorValues, uint8_t ID_profile)
|
||||
{
|
||||
//*Do something smart*//
|
||||
|
||||
switch (ID_profile)
|
||||
{
|
||||
case 1:
|
||||
sensorValues[ROLL] = 0;
|
||||
sensorValues[PITCH] = 0;
|
||||
sensorValues[YAW] = 0;
|
||||
break;
|
||||
case 2:
|
||||
break;
|
||||
case 3:
|
||||
break;
|
||||
case 4:
|
||||
break;
|
||||
case 5:
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Update desired values from rc command *
|
||||
* INFORMATION: *
|
||||
**************************************************************************/
|
||||
void getPointRate(float *desiredCommand, uint8_t ID_profile)
|
||||
{
|
||||
//*Do something smart*//
|
||||
switch (ID_profile)
|
||||
{
|
||||
case 1:
|
||||
desiredCommand[ROLL] = 0;
|
||||
desiredCommand[PITCH] = 0;
|
||||
desiredCommand[YAW] = 0;
|
||||
break;
|
||||
case 2:
|
||||
break;
|
||||
case 3:
|
||||
break;
|
||||
case 4:
|
||||
break;
|
||||
case 5:
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Initializes the pid profile PID controller *
|
||||
* INFORMATION: Recommended to use if unexpected values occur of profile *
|
||||
**************************************************************************/
|
||||
void pidUAVInit(pidProfile_t *pidProfile)
|
||||
{
|
||||
/*This is recommended init settings*/
|
||||
pidProfile->ID_profile = 0;
|
||||
|
||||
pidProfile-> pidStabilisationEnabled = true;
|
||||
|
||||
pidProfile->DOF = 0;
|
||||
|
||||
pidProfile->P[ROLL] = 10;
|
||||
pidProfile->P[PITCH] = 10;
|
||||
pidProfile->P[YAW] = 10;
|
||||
|
||||
pidProfile->I[ROLL] = 0;
|
||||
pidProfile->I[PITCH] = 0;
|
||||
pidProfile->I[YAW] = 0;
|
||||
|
||||
pidProfile->D[ROLL] = 0;
|
||||
pidProfile->D[PITCH] = 0;
|
||||
pidProfile->D[YAW] = 0;
|
||||
|
||||
pidProfile->Max_PID_Term = 3000;
|
||||
pidProfile->Max_Error_Term = 5000;
|
||||
|
||||
pidProfile->Max_P_Term = 0;
|
||||
pidProfile->Max_I_Term = 0;
|
||||
pidProfile->Max_D_Term = 0;
|
||||
|
||||
|
||||
pidProfile->dterm_lpf = 0;
|
||||
pidProfile->yaw_lpf = 0;
|
||||
|
||||
pidProfile->Integral[ROLL] = 0;
|
||||
pidProfile->Integral[PITCH] = 0;
|
||||
pidProfile->Integral[YAW] = 0;
|
||||
|
||||
pidProfile->LastError[ROLL] = 0;
|
||||
pidProfile->LastError[PITCH] = 0;
|
||||
pidProfile->LastError[YAW] = 0;
|
||||
|
||||
pidProfile->PID_Out[ROLL] = 0;
|
||||
pidProfile->PID_Out[PITCH] = 0;
|
||||
pidProfile->PID_Out[YAW] = 0;
|
||||
|
||||
pidProfile->deltaFilter[ROLL].RC = 0;
|
||||
pidProfile->deltaFilter[ROLL].dT = 0;
|
||||
pidProfile->deltaFilter[ROLL].state = 0;
|
||||
|
||||
pidProfile->deltaFilter[PITCH].RC = 0;
|
||||
pidProfile->deltaFilter[PITCH].dT = 0;
|
||||
pidProfile->deltaFilter[PITCH].state = 0;
|
||||
|
||||
pidProfile->deltaFilter[YAW].RC = 0;
|
||||
pidProfile->deltaFilter[YAW].dT = 0;
|
||||
pidProfile->deltaFilter[YAW].state = 0;
|
||||
|
||||
pidProfile->yawFilter.RC = 0;
|
||||
pidProfile->yawFilter.dT = 0;
|
||||
pidProfile->yawFilter.state = 0;
|
||||
|
||||
pidProfile->dT = 1;
|
||||
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Dynamic PID controller, able to handle several PID controller *
|
||||
* connected to different profiles.
|
||||
* INFORMATION: *
|
||||
**************************************************************************/
|
||||
void pidUAV(pidProfile_t *pidProfile)
|
||||
{
|
||||
int DOF = pidProfile->DOF;
|
||||
|
||||
float errorRate;
|
||||
float Derivative;
|
||||
|
||||
float ITerm, PTerm, DTerm;
|
||||
float PID_Out[XYZ_AXIS_COUNT];
|
||||
|
||||
float errorAxis[3] = {0}; /*Array of errors for each axis*/
|
||||
float pointRate[3] = {0}; /*Array of desired values for each axis*/
|
||||
|
||||
/*PID values*/
|
||||
float Kp[3] = {0};
|
||||
float Ki[3] = {0};
|
||||
float Kd[3] = {0};
|
||||
|
||||
getCurrentValues(errorAxis, pidProfile->ID_profile); /*Get sensor values*/
|
||||
getPointRate(pointRate, pidProfile->ID_profile); /*Get reference values or desired values*/
|
||||
|
||||
/* -------------PID controller------------- */
|
||||
for (int axis = 0; axis < DOF; axis++)
|
||||
{
|
||||
|
||||
errorRate = pointRate[axis] - errorAxis[axis]; /*Calculates current error of one axis*/
|
||||
errorRate = constrainf(errorRate, -pidProfile->Max_Error_Term, pidProfile->Max_Error_Term); /*Limits the error if necessary*/
|
||||
|
||||
Kp[axis] = PTERM_SCALE * pidProfile->P[axis]; /*Calculates Kp, Ki, Kd value with scale values*/
|
||||
Ki[axis] = ITERM_SCALE * pidProfile->I[axis];
|
||||
Kd[axis] = DTERM_SCALE * pidProfile->D[axis];
|
||||
|
||||
|
||||
/*--------P term--------*/
|
||||
PTerm = Kp[axis] * errorRate;
|
||||
|
||||
if (axis == YAW)
|
||||
{
|
||||
/*Low pass filter on P term for Yaw axis*/
|
||||
if (pidProfile->yaw_lpf)
|
||||
{
|
||||
PTerm = pt1FilterApply4(&pidProfile->yawFilter, PTerm, pidProfile->yaw_lpf, pidProfile->dT);
|
||||
}
|
||||
|
||||
/*Constrains PTerm if needed*/
|
||||
if (pidProfile->Max_P_Term)
|
||||
{
|
||||
PTerm = constrainf(PTerm, -pidProfile->Max_P_Term, pidProfile->Max_P_Term);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*--------I term--------*/
|
||||
pidProfile->Integral[axis] = pidProfile->Integral[axis] + errorRate;
|
||||
ITerm = Ki[axis] * pidProfile->Integral[axis];
|
||||
|
||||
/*Limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated*/
|
||||
if (pidProfile->Max_I_Term)
|
||||
{
|
||||
ITerm = constrainf(ITerm, -pidProfile->Max_I_Term, pidProfile->Max_I_Term);
|
||||
}
|
||||
|
||||
|
||||
/*--------D term--------*/
|
||||
Derivative = errorRate - pidProfile->LastError[axis];
|
||||
pidProfile->LastError[axis] = errorRate;
|
||||
|
||||
/*Divide delta by dT to get differential (ie dr/dt)*/
|
||||
Derivative *= (1.0f / pidProfile->dT);
|
||||
|
||||
/*DTerm delta low pass filter*/
|
||||
if (pidProfile->dterm_lpf)
|
||||
{
|
||||
Derivative = pt1FilterApply4(&pidProfile->deltaFilter[axis], Derivative, pidProfile->dterm_lpf, pidProfile->dT);
|
||||
}
|
||||
|
||||
DTerm = Kd[axis] * Derivative;
|
||||
|
||||
|
||||
/* --------------PID Out ----------------- */
|
||||
PID_Out[axis] = PTerm + ITerm + DTerm;
|
||||
|
||||
/*Constrains PID out term*/
|
||||
PID_Out[axis] = constrainf(PID_Out[axis], -pidProfile->Max_PID_Term, pidProfile->Max_PID_Term);
|
||||
|
||||
if (!pidProfile->pidStabilisationEnabled) PID_Out[axis] = 0;
|
||||
|
||||
/*Update PID out values to profile*/
|
||||
pidProfile->PID_Out[axis] = PID_Out[axis];
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user