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

Flight/Stabilization: Convert all PID loops to units of seconds instead of ms.

Browse Source 
THIS MEANS YOU MUST RETUNE YOUR PID LOOP IF YOU'RE FLYING WITH IT

Alternatively divide Kd by 1000 and multiply Ki by 1000

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1782 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-27 09:04:43 +00:00 committed by peabody124
parent c5863320fe
commit aa42256ad1
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -112,16 +112,16 @@ static void stabilizationTask(void* parameters)
// Pitch stabilization control loop
pitchError = attitudeDesired.Pitch - attitudeActual.Pitch;
pitchDerivative = (pitchError - pitchErrorLast) / stabSettings.UpdatePeriod;
pitchIntegral = bound(pitchIntegral+pitchError*stabSettings.UpdatePeriod, -stabSettings.PitchIntegralLimit, stabSettings.PitchIntegralLimit);
pitchDerivative = ((pitchError - pitchErrorLast) * 1000) / stabSettings.UpdatePeriod;
pitchIntegral = bound(pitchIntegral+(pitchError * stabSettings.UpdatePeriod) / 1000, -stabSettings.PitchIntegralLimit, stabSettings.PitchIntegralLimit);
actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);
pitchErrorLast = pitchError;
// Roll stabilization control loop
rollError = attitudeDesired.Roll - attitudeActual.Roll;
rollDerivative = (rollError - rollErrorLast) / stabSettings.UpdatePeriod;
rollIntegral = bound(rollIntegral+rollError*stabSettings.UpdatePeriod, -stabSettings.RollIntegralLimit, stabSettings.RollIntegralLimit);
rollDerivative = ((rollError - rollErrorLast) * 1000) / stabSettings.UpdatePeriod;
rollIntegral = bound(rollIntegral+(rollError * stabSettings.UpdatePeriod) / 1000, -stabSettings.RollIntegralLimit, stabSettings.RollIntegralLimit);
actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;
actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
rollErrorLast = rollError;
@ -130,7 +130,7 @@ static void stabilizationTask(void* parameters)
if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
{
if(stabSettings.YawMode == STABILIZATIONSETTINGS_YAWMODE_RATE) { // rate stabilization on yaw
yawChange = (attitudeActual.Yaw - yawPrevious) * 1000 / stabSettings.UpdatePeriod;
yawChange = ((attitudeActual.Yaw - yawPrevious) * 1000) / stabSettings.UpdatePeriod;
yawPrevious = attitudeActual.Yaw;
yawError = bound(attitudeDesired.Yaw, -stabSettings.YawMax, stabSettings.YawMax) - yawChange;
} else { // heading stabilization
@ -140,8 +140,8 @@ static void stabilizationTask(void* parameters)
//this should make it take the quickest path to reach the desired yaw
if (yawError>180.0)yawError -= 360;
if (yawError<-180.0)yawError += 360;
yawDerivative = (yawError - yawErrorLast) / stabSettings.UpdatePeriod;
yawIntegral = bound(yawIntegral+yawError*stabSettings.UpdatePeriod, -stabSettings.YawIntegralLimit, stabSettings.YawIntegralLimit);
yawDerivative = ((yawError - yawErrorLast) * 1000) / stabSettings.UpdatePeriod;
yawIntegral = bound(yawIntegral+(yawError * stabSettings.UpdatePeriod) / 1000, -stabSettings.YawIntegralLimit, stabSettings.YawIntegralLimit);
actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
yawErrorLast = yawError;