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

Flight/Guidance: Convert PositionPID mode to PositionPI + Velocity feedback.

Browse Source 
Needs AHRS with accel_bias estimation to behave well.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2187 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-12-04 17:34:19 +00:00 committed by peabody124
parent 0679e8d472
commit e861bcb9d0
1 changed files with 11 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
flight/OpenPilot/Modules/Guidance/guidance.c
View File

@ -299,17 +299,16 @@ static void positionPIDcontrol()
AttitudeDesiredData attitudeDesired;
AttitudeActualData attitudeActual;
GuidanceSettingsData guidanceSettings;
VelocityActualData velocityActual;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
PositionActualData positionActual;
PositionDesiredData positionDesired;
float northError;
float northDerivative;
float northCommand;
float eastError;
float eastDerivative;
float eastCommand;
// Check how long since last update
@ -325,26 +324,26 @@ static void positionPIDcontrol()
StabilizationSettingsGet(&stabSettings);
PositionActualGet(&positionActual);
PositionDesiredGet(&positionDesired);
VelocityActualGet(&velocityActual);
attitudeDesired.Yaw = 0; // try and face north
// Yaw and pitch output from ground speed PID loop
northError = positionDesired.North - positionActual.North;
northDerivative = (northError - northErrorLast) / dT;
northIntegral =
bound(northIntegral + northError * dT, -guidanceSettings.MaxVelIntegral,
guidanceSettings.MaxVelIntegral);
northErrorLast = northError;
northCommand =
northError * guidanceSettings.VelP + northDerivative * guidanceSettings.VelD + northIntegral * guidanceSettings.VelI;
northIntegral = bound(northIntegral + northError * dT,
-guidanceSettings.MaxVelIntegral,
guidanceSettings.MaxVelIntegral);
northCommand = northError * guidanceSettings.VelP +
northIntegral * guidanceSettings.VelI -
velocityActual.North * guidanceSettings.VelD;
eastError = positionDesired.East - positionActual.East;
eastDerivative = (eastError - eastErrorLast) / dT;
eastIntegral = bound(eastIntegral + eastError * dT,
-guidanceSettings.MaxVelIntegral,
guidanceSettings.MaxVelIntegral);
eastErrorLast = eastError;
eastCommand = eastError * guidanceSettings.VelP + eastDerivative * guidanceSettings.VelD + eastIntegral * guidanceSettings.VelI;
eastCommand = eastError * guidanceSettings.VelP +
eastIntegral * guidanceSettings.VelI -
velocityActual.East * guidanceSettings.VelD;
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
// craft should move similarly for 5 deg roll versus 5 deg pitch