rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +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; AttitudeDesiredData attitudeDesired;
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
GuidanceSettingsData guidanceSettings; GuidanceSettingsData guidanceSettings;
VelocityActualData velocityActual;
StabilizationSettingsData stabSettings; StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings; SystemSettingsData systemSettings;
PositionActualData positionActual; PositionActualData positionActual;
PositionDesiredData positionDesired; PositionDesiredData positionDesired;
float northError; float northError;
float northDerivative;
float northCommand; float northCommand;
float eastError; float eastError;
float eastDerivative;
float eastCommand; float eastCommand;
// Check how long since last update // Check how long since last update
@ -325,26 +324,26 @@ static void positionPIDcontrol()
StabilizationSettingsGet(&stabSettings); StabilizationSettingsGet(&stabSettings);
PositionActualGet(&positionActual); PositionActualGet(&positionActual);
PositionDesiredGet(&positionDesired); PositionDesiredGet(&positionDesired);
VelocityActualGet(&velocityActual);
attitudeDesired.Yaw = 0; // try and face north attitudeDesired.Yaw = 0; // try and face north
// Yaw and pitch output from ground speed PID loop // Yaw and pitch output from ground speed PID loop
northError = positionDesired.North - positionActual.North; northError = positionDesired.North - positionActual.North;
northDerivative = (northError - northErrorLast) / dT; northIntegral = bound(northIntegral + northError * dT,
northIntegral = -guidanceSettings.MaxVelIntegral,
bound(northIntegral + northError * dT, -guidanceSettings.MaxVelIntegral, guidanceSettings.MaxVelIntegral);
guidanceSettings.MaxVelIntegral); northCommand = northError * guidanceSettings.VelP +
northErrorLast = northError; northIntegral * guidanceSettings.VelI -
northCommand = velocityActual.North * guidanceSettings.VelD;
northError * guidanceSettings.VelP + northDerivative * guidanceSettings.VelD + northIntegral * guidanceSettings.VelI;
eastError = positionDesired.East - positionActual.East; eastError = positionDesired.East - positionActual.East;
eastDerivative = (eastError - eastErrorLast) / dT;
eastIntegral = bound(eastIntegral + eastError * dT, eastIntegral = bound(eastIntegral + eastError * dT,
-guidanceSettings.MaxVelIntegral, -guidanceSettings.MaxVelIntegral,
guidanceSettings.MaxVelIntegral); guidanceSettings.MaxVelIntegral);
eastErrorLast = eastError; eastCommand = eastError * guidanceSettings.VelP +
eastCommand = eastError * guidanceSettings.VelP + eastDerivative * guidanceSettings.VelD + eastIntegral * guidanceSettings.VelI; 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 // 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 // craft should move similarly for 5 deg roll versus 5 deg pitch