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

OP-1483 implement leaky integral on position control loop in pathfollower (vtol only)

Browse Source
This commit is contained in:
Corvus Corax 2014-09-19 15:52:11 +02:00
parent c90973cb99
commit 1a0ab29050
2 changed files with 16 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
flight/modules/PathFollower/pathfollower.c
View File

@ -90,8 +90,10 @@
struct Globals {
struct pid PIDposH[2];
float PIDposHLeakAlpha;
struct pid PIDposV;
struct pid PIDvel[3];
float PIDposVLeakAlpha;
struct pid PIDcourse;
struct pid PIDspeed;
struct pid PIDpower;
@ -345,7 +347,9 @@ static uint8_t updateAutoPilotFixedWing()
{
pid_configure(&global.PIDposH[0], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposH[1], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
global.PIDposHLeakAlpha = 1.0f;
pid_configure(&global.PIDposV, fixedWingPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f);
global.PIDposVLeakAlpha = 1.0f;
updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, true);
return updateFixedDesiredAttitude();
}
@ -376,14 +380,16 @@ static uint8_t updateAutoPilotVtol()
}
// vertical positon control PID loops works the same in both regular and fallback modes, setup according to settings
pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.VerticalPosPI.Ki, 0.0f, vtolPathFollowerSettings.VerticalPosPI.ILimit);
global.PIDposVLeakAlpha = vtolPathFollowerSettings.VerticalPosPI.ILeak;
switch (followermode) {
case FOLLOWER_REGULAR:
{
// horizontal position control PID loop works according to settings in regular mode, allowing integral terms
pid_configure(&global.PIDposH[0], vtolPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposH[1], vtolPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposH[0], vtolPathFollowerSettings.HorizontalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit);
pid_configure(&global.PIDposH[1], vtolPathFollowerSettings.HorizontalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit);
global.PIDposHLeakAlpha = vtolPathFollowerSettings.HorizontalPosPI.ILeak;
updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, false);
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
@ -419,6 +425,7 @@ static uint8_t updateAutoPilotVtol()
// fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly
pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f);
global.PIDposVLeakAlpha = 1.0f;
updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true);
// emergency follower has no return value
@ -655,11 +662,14 @@ static void updatePathVelocity(float kFF, bool limited)
// calculate correction
velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT);
velocityDesired.East += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT);
global.PIDposH[0].iAccumulator *= global.PIDposHLeakAlpha;
global.PIDposH[1].iAccumulator *= global.PIDposHLeakAlpha;
}
velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT);
global.PIDposV.iAccumulator *= global.PIDposVLeakAlpha;
// update pathstatus
pathStatus.error = progress.error;
pathStatus.error = progress.error;
pathStatus.fractional_progress = progress.fractional_progress;
pathStatus.path_direction_north = progress.path_vector[0];
pathStatus.path_direction_east = progress.path_vector[1];

4
shared/uavobjectdefinition/vtolpathfollowersettings.xml
View File

@ -5,8 +5,8 @@
<field name="HorizontalVelMax" units="m/s" type="float" elements="1" defaultvalue="5.0"/>
<field name="VerticalVelMax" units="m/s" type="float" elements="1" defaultvalue="1.0"/>
<field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="1.0"/>
<field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.25"/>
<field name="VerticalPosP" units="" type="float" elements="1" defaultvalue="0.4"/>
<field name="HorizontalPosPI" units="(m/s)/m" type="float" elementnames="Kp,Ki,ILimit,ILeak" defaultvalue="0.25,0,0,1.0"/>
<field name="VerticalPosPI" units="" type="float" elementnames="Kp,Ki,ILimit,ILeak" defaultvalue="0.4,0,0,1.0"/>
<field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="8.0, 0.5, 0.0, 15"/>
<field name="VerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.1, 0.01, 0.0, 1.0"/>
<field name="ThrustLimits" units="" type="float" elementnames="Min,Neutral,Max" defaultvalue="0.2, 0.5, 0.9"/>