Skip to content

Cruise Controller FLC refactored using PID #36

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
T2Fat2Fly wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Cruise Controller FLC refactored using PID #36

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
86 changes: 52 additions & 34 deletions AtmosphereAutopilot/Modules/CruiseController.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,13 @@ public sealed class CruiseController : StateController
{
internal CruiseController(Vessel v)
: base(v, "Cruise Flight controller", 88437226)
{ }
{
flc_controller.KP = 0.6;
flc_controller.KI = 0.03;
flc_controller.KD = 0.2;
flc_controller.IntegralClamp = double.PositiveInfinity;
Copy link
Owner

@Boris-Barboris Boris-Barboris Nov 9, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not a fan of infinities here. If the controller's output is logically bound to [-90; 90], we can dynamically choose some sane clamp value (derived from current KI value). If not, crafts that are very very fat and approach desired climb angle very slowly, may end up with an enormous accumulator value that will cause FLC to overshoot.
Set some limit. It needs not to be rigorous or universal, but it should be some sane number.

Copy link
Contributor Author

@T2Fat2Fly T2Fat2Fly Nov 10, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

TBH I have trouble understanding these clamps (sorry I'm (very!) new to the industrial controllers).

If I understand correctly, the IntergralClamp is used to filter out inputs that is off the setpoint too much. But this is not the case where actual airspeed can be anywhere, regardless of where the setpoint is. So I set it to infinity.

For the AccumulatorClamp, you are right about it should be clamped. Maybe setting it to 1.0 / KI just like you did it in the thrust PID controller? I still have no idea about how to tweak this value.

Copy link
Owner

@Boris-Barboris Boris-Barboris Nov 10, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would try:
accumulator clamp = 90.0 / KI
AccumulDerivClamp = 2

Copy link
Owner

@Boris-Barboris Boris-Barboris Nov 10, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

IntegralClamp as infinity should probably be fine

flc_controller.AccumulatorClamp = double.PositiveInfinity;
}

FlightModel imodel;
DirectorController dir_c;
Expand Down Expand Up @@ -229,14 +235,24 @@ public enum HeightMode
[VesselSerializable("desired_vertspeed_field")]
public DelayedFieldFloat desired_vertspeed = new DelayedFieldFloat(0.0f, "G4");

[GlobalSerializable("preudo_flc")]
[VesselSerializable("preudo_flc")]
[AutoGuiAttr("pseudo-FLC", true)]
public bool pseudo_flc = true;
[GlobalSerializable("pseudo_flc")]
[VesselSerializable("pseudo_flc")]
[AutoGuiAttr("pseudo_flc", true)]
public bool pseudo_flc = false;

PIDController flc_controller = new PIDController();

[VesselSerializable("flc_pid_Kp")]
[AutoGuiAttr("flc_pid_Kp", true, "G4")]
public double flc_pid_Kp { get { return flc_controller.KP; } set { flc_controller.KP = value; } }

[VesselSerializable("flc_margin")]
[AutoGuiAttr("flc_margin", true, "G4")]
public double flc_margin = 20.0;
[VesselSerializable("flc_pid_Ki")]
[AutoGuiAttr("flc_pid_Ki", true, "G4")]
public double flc_pid_Ki { get { return flc_controller.KI; } set { flc_controller.KI = value; } }

[VesselSerializable("flc_pid_Kd")]
[AutoGuiAttr("flc_pid_Kd", true, "G4")]
public double flc_pid_Kd { get { return flc_controller.KD; } set { flc_controller.KD = value; } }

[VesselSerializable("strength_mult")]
[AutoGuiAttr("strength_mult", true, "G5")]
Expand Down Expand Up @@ -278,6 +294,10 @@ Vector3d account_for_height(Vector3d desired_direction)

Vector3d proportional_acc = Vector3d.zero;
double cur_vert_speed = Vector3d.Dot(imodel.surface_v, planet2vesNorm);

// speed control portion for ascent\descent
double effective_max_climb_angle = max_climb_angle;

if (Math.Abs(height_error) < height_relax_frame)
{
relax_transition_k = Common.Clamp(2.0 * (height_relax_frame - Math.Abs(height_error)), 0.0, 1.0);
Expand All @@ -286,6 +306,30 @@ Vector3d account_for_height(Vector3d desired_direction)
// exponential descent
if (cur_vert_speed * height_error > 0.0)
proportional_acc = -planet2vesNorm * height_relax_Kp * cur_vert_speed;
flc_controller.clear();
}
else if (pseudo_flc)
{
// refactored flc using pid loop
double flc_pid_control = flc_controller.Control(imodel.surface_v_magnitude, thrust_c.setpoint.mps(), TimeWarp.fixedDeltaTime);
if (height_error >= 0.0)
{
effective_max_climb_angle = Common.Clamp(-flc_pid_control, 0.1, max_climb_angle);
if (thrust_c.spd_control_enabled)
thrust_c.ForceThrottle(1.0f);
}
else
{
effective_max_climb_angle = Common.Clamp(-flc_pid_control, -max_climb_angle, -0.1);
if (thrust_c.spd_control_enabled)
thrust_c.ForceThrottle(0.0f);
}

}
else
{
effective_max_climb_angle *= Math.Max(0.0, Math.Min(1.0, vessel.srfSpeed / thrust_c.setpoint.mps()));
flc_controller.clear();
}

// let's assume parabolic ascent\descend
Expand All @@ -304,32 +348,6 @@ Vector3d account_for_height(Vector3d desired_direction)
parabolic_acc = -planet2vesNorm * 0.5 * cur_vert_speed * cur_vert_speed / height_error;
}

// speed control portion for ascend
double effective_max_climb_angle = max_climb_angle;
if (thrust_c.spd_control_enabled && (height_error >= 0.0))
{
if (pseudo_flc)
{
filtered_drag = Common.simple_filter(thrust_c.drag_estimate, filtered_drag, 5.0);
if (thrust_c.estimated_max_thrust > filtered_drag)
{
double sin = Vector3d.Dot(-planet2vesNorm, imodel.gravity_acc + imodel.noninert_acc) / (thrust_c.estimated_max_thrust - filtered_drag);
if (sin < 0.0 || sin >= 1.0)
effective_max_climb_angle = Math.Min(Math.Asin(sin), max_climb_angle);
}
else
effective_max_climb_angle = 1.0;

double spd_diff = (imodel.surface_v_magnitude - thrust_c.setpoint.mps());
if (spd_diff < -flc_margin)
effective_max_climb_angle *= 0.0;
else if (spd_diff < 0.0)
effective_max_climb_angle *= (spd_diff + flc_margin) / flc_margin;
}
else
effective_max_climb_angle *= Math.Max(0.0, Math.Min(1.0, vessel.srfSpeed / thrust_c.setpoint.mps()));
}

double max_vert_speed = vessel.horizontalSrfSpeed * Math.Tan(effective_max_climb_angle * dgr2rad);
bool apply_acc = Math.Abs(des_vert_speed) < max_vert_speed;
des_vert_speed = Common.Clamp(des_vert_speed, max_vert_speed);
Expand Down
29 changes: 29 additions & 0 deletions AtmosphereAutopilot/Modules/ProgradeThrustController.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -176,13 +176,42 @@ protected override void OnDeactivate()

private bool was_breaking_previously = true;

private bool is_throttle_forced = false;
private float forced_throttle = 0.0f;

/// <summary>
/// Allow other module to force throttle output in the next frame
/// </summary>
/// <param name="throttle">Desired throttle for this frame</param>
/// <returns>Achievable throttle regarding the desired throttle</returns>
public float ForceThrottle(float throttle)
{
is_throttle_forced = true;
forced_throttle = Common.Clampf(throttle, 0.0f, 1.0f);
// Currently throttle output is always the same to the desired input
return throttle;
}

/// <summary>
/// Main control function
/// </summary>
/// <param name="cntrl">Control state to change</param>
/// <param name="target_value">Prograde surface speed setpoint</param>
public override float ApplyControl(FlightCtrlState cntrl, float target_value)
{
if (is_throttle_forced)
{
if (use_breaks && was_breaking_previously)
{
vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, false);
was_breaking_previously = false;
}

is_throttle_forced = false;
cntrl.mainThrottle = forced_throttle;
return cntrl.mainThrottle;
}

current_v = imodel.surface_v_magnitude;
desired_v = target_value;

Expand Down