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

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

Browse Source 
This reverts commit 1a0ab290506f20854f171f36ad5e973e103464b6.
This commit is contained in:
Corvus Corax 2014-09-20 13:45:45 +02:00
parent b315c8f106
commit 76f94e13ef
2 changed files with 6 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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="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="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="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="1.0"/>
<field name="HorizontalPosPI" units="(m/s)/m" type="float" elementnames="Kp,Ki,ILimit,ILeak" defaultvalue="0.25,0,0,1.0"/> <field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.25"/>
<field name="VerticalPosPI" units="" type="float" elementnames="Kp,Ki,ILimit,ILeak" defaultvalue="0.4,0,0,1.0"/> <field name="VerticalPosP" units="" type="float" elements="1" defaultvalue="0.4"/>
<field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="8.0, 0.5, 0.0, 15"/> <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="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"/> <field name="ThrustLimits" units="" type="float" elementnames="Min,Neutral,Max" defaultvalue="0.2, 0.5, 0.9"/>