rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Drop 8 bytes per PID of unecessary information. Also trying to compute

Browse Source 
integral in ms instead of seconds to avoid rounding errors.
This commit is contained in:
James Cotton 2011-06-19 11:12:54 -05:00
parent 80709f356f
commit 9c1e523260
1 changed files with 7 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
flight/Modules/Stabilization/stabilization.c
View File

@ -66,10 +66,8 @@ enum {ROLL,PITCH,YAW,MAX_AXES};
typedef struct { typedef struct {
float p; float p;
float i; float i;
float d;
float iLim; float iLim;
float iAccumulator; float iAccumulator;
float lastErr;
} pid_type; } pid_type;
// Private variables // Private variables
@ -287,17 +285,14 @@ static void stabilizationTask(void* parameters)
float ApplyPid(pid_type * pid, const float err) float ApplyPid(pid_type * pid, const float err)
{ {
float diff = (err - pid->lastErr); // Scale up accumulator by 1000 while computing to avoid losing precision
pid->lastErr = err; pid->iAccumulator += err * (pid->i * dT * 1000);
pid->iAccumulator += err * pid->i * dT; if(pid->iAccumulator > (pid->iLim * 1000)) {
if(fabs(pid->iAccumulator) > pid->iLim) { pid->iAccumulator = pid->iLim * 1000;
if(pid->iAccumulator >0) { } else if (pid->iAccumulator < -(pid->iLim * 1000)) {
pid->iAccumulator = pid->iLim; pid->iAccumulator = -pid->iLim * 1000;
} else {
pid->iAccumulator = -pid->iLim;
}
} }
return ((err * pid->p) + pid->iAccumulator + (diff * pid->d / dT)); return (err * pid->p) + pid->iAccumulator / 1000;
} }
@ -305,7 +300,6 @@ static void ZeroPids(void)
{ {
for(int8_t ct = 0; ct < PID_MAX; ct++) { for(int8_t ct = 0; ct < PID_MAX; ct++) {
pids[ct].iAccumulator = 0; pids[ct].iAccumulator = 0;
pids[ct].lastErr = 0;
} }
} }