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

PID new test changes, cast I term to float

Browse Source
This commit is contained in:
johan9107 2016-10-31 09:58:04 +01:00
parent df965c19b3
commit dab5b5626d
1 changed files with 17 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
UAV-ControlSystem/src/Flight/pid.c
View File

@ -18,6 +18,8 @@
#include "scheduler/scheduler.h" #include "scheduler/scheduler.h"
#include <math.h> #include <math.h>
#include "drivers/failsafe_toggles.h" #include "drivers/failsafe_toggles.h"
#include "drivers/motormix.h"
#define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ #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 ITERM_SCALE 0.244381f /*I-term used as a scale value to the PID controller*/
@ -29,7 +31,7 @@
#define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/ #define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/
#define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/ #define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/
#define PID_MAX_I 256 /*Constrains ITerm*/ #define PID_MAX_I 256/2 /*Constrains ITerm*/
#define PID_MAX_D 512 /*Constrains DTerm*/ #define PID_MAX_D 512 /*Constrains DTerm*/
/*Struct that belongs to a certain PID controller*/ /*Struct that belongs to a certain PID controller*/
@ -117,8 +119,8 @@ float constrainf(float amt, int low, int high)
float oldSensorValue[2] = {0}; float oldSensorValue[2] = {0};
float oldSensorValueRoll[50] = {0}; float oldSensorValueRoll[20] = {0};
float oldSensorValuePitch[50] = {0}; float oldSensorValuePitch[20] = {0};
int i = 0; int i = 0;
@ -164,7 +166,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
float RollValue = 0; float RollValue = 0;
float PitchValue = 0; float PitchValue = 0;
for (int ii = 0; ii < 50; ii++) for (int ii = 0; ii < 20; ii++)
{ {
RollValue = RollValue + oldSensorValueRoll[ii]; RollValue = RollValue + oldSensorValueRoll[ii];
PitchValue = PitchValue + oldSensorValuePitch[ii]; PitchValue = PitchValue + oldSensorValuePitch[ii];
@ -172,13 +174,13 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
} }
i = (i < 49)? i + 1:0; i = (i < 19)? i + 1:0;
sensorValues[ROLL] = RollValue/50; sensorValues[ROLL] = RollValue/20;
sensorValues[PITCH] = PitchValue/50; sensorValues[PITCH] = PitchValue/20;
sensorValues[ROLL] = alpha*RollValue/50 + (1-alpha)*oldSensorValue[0]; sensorValues[ROLL] = alpha*RollValue/20 + (1-alpha)*oldSensorValue[0];
sensorValues[PITCH] = alpha*PitchValue/50 + (1-alpha)*oldSensorValue[1]; sensorValues[PITCH] = alpha*PitchValue/20 + (1-alpha)*oldSensorValue[1];
// //
oldSensorValue[0] = sensorValues[ROLL]; oldSensorValue[0] = sensorValues[ROLL];
oldSensorValue[1] = sensorValues[PITCH]; oldSensorValue[1] = sensorValues[PITCH];
@ -291,7 +293,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
/* -----calculate I component ---- */ /* -----calculate I component ---- */
float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * pidProfile->I[axis]; float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis];
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I) moved before integration to make limiting independent from PID settings // I coefficient (I) moved before integration to make limiting independent from PID settings
@ -341,6 +343,11 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
/*----PID OUT----*/ /*----PID OUT----*/
if(!flags_IsSet_ID(systemFlags_armed_id) || (rc_input.Throttle < mixerConfig.minCheck))
{
ITerm = 0;
pidProfileBuff->lastITerm[axis] = 0;
}
pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -pidProfile->pid_out_limit, pidProfile->pid_out_limit); pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -pidProfile->pid_out_limit, pidProfile->pid_out_limit);
} }