From f0c400552db13e06ac26b266c74fdc84f827e2ee Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 12 Oct 2016 17:17:01 +0200 Subject: [PATCH] pid and low pass filter Added pid controller functions and low pass filter functions --- UAV-ControlSystem/inc/Flight/filter.h | 37 ++++ UAV-ControlSystem/inc/Flight/pid.h | 97 ++++++++++ UAV-ControlSystem/src/Flight/filter.c | 31 ++++ UAV-ControlSystem/src/Flight/pid.c | 251 ++++++++++++++++++++++++++ 4 files changed, 416 insertions(+) create mode 100644 UAV-ControlSystem/inc/Flight/filter.h create mode 100644 UAV-ControlSystem/inc/Flight/pid.h create mode 100644 UAV-ControlSystem/src/Flight/filter.c create mode 100644 UAV-ControlSystem/src/Flight/pid.c diff --git a/UAV-ControlSystem/inc/Flight/filter.h b/UAV-ControlSystem/inc/Flight/filter.h new file mode 100644 index 0000000..5279acb --- /dev/null +++ b/UAV-ControlSystem/inc/Flight/filter.h @@ -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 +#include + +/*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_ */ diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h new file mode 100644 index 0000000..04cc152 --- /dev/null +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -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 +#include + +#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_ */ diff --git a/UAV-ControlSystem/src/Flight/filter.c b/UAV-ControlSystem/src/Flight/filter.c new file mode 100644 index 0000000..5a15ae7 --- /dev/null +++ b/UAV-ControlSystem/src/Flight/filter.c @@ -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; +} diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c new file mode 100644 index 0000000..494376f --- /dev/null +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -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]; + } +}