philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #15 from MDHSweden/PID

Browse Source 
pid and low pass filter Accepted by Lennart Eriksson
This commit is contained in:
Lennart Eriksson 2016-10-12 17:40:30 +02:00 committed by GitHub
commit 62d2701e9e
4 changed files with 416 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
UAV-ControlSystem/inc/Flight/filter.h Normal file
View 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
View 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
View 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
View 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];
}
}