rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-30 08:24:11 +01:00
Code Issues Releases Activity

Changed rate controller from PID to PIDT1 to improve flight stability.

Browse Source 
Low pass on D-Term makes the D-Term usable to improve flight stability.
Affects rate and stabilize mode.
This commit is contained in:
Igor Van Airde 2012-09-09 13:07:25 +02:00
parent 2a4e135c81
commit 8ca99739ed
1 changed files with 13 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -68,6 +68,7 @@ typedef struct {
float iLim; float iLim;
float iAccumulator; float iAccumulator;
float lastErr; float lastErr;
float lastDer;
} pid_type; } pid_type;
// Private variables // Private variables
@ -405,6 +406,7 @@ static void stabilizationTask(void* parameters)
float ApplyPid(pid_type * pid, const float err, float dT) float ApplyPid(pid_type * pid, const float err, float dT)
{ {
float diff = (err - pid->lastErr); float diff = (err - pid->lastErr);
float dterm = 0;
pid->lastErr = err; pid->lastErr = err;
// Scale up accumulator by 1000 while computing to avoid losing precision // Scale up accumulator by 1000 while computing to avoid losing precision
@ -414,7 +416,16 @@ float ApplyPid(pid_type * pid, const float err, float dT)
} else if (pid->iAccumulator < -(pid->iLim * 1000.0f)) { } else if (pid->iAccumulator < -(pid->iLim * 1000.0f)) {
pid->iAccumulator = -pid->iLim * 1000.0f; pid->iAccumulator = -pid->iLim * 1000.0f;
} }
return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT));
// Calculate DT1 term, fixed T1 timeconstant
if(pid->d)
{
dterm = pid->lastDer + (( dT / ( dT + 7.9577e-3)) * ((diff * pid->d / dT) - pid->lastDer));
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
// 7.9577e-3 means 20 Hz f_cutoff
}
return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm);
} }
@ -423,6 +434,7 @@ 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.0f; pids[ct].iAccumulator = 0.0f;
pids[ct].lastErr = 0.0f; pids[ct].lastErr = 0.0f;
pids[ct].lastDer = 0.0f;
} }
for(uint8_t i = 0; i < 3; i++) for(uint8_t i = 0; i < 3; i++)
axis_lock_accum[i] = 0.0f; axis_lock_accum[i] = 0.0f;