public void InitReentryBurn(float kP, float maxAngle, double alt, double tgtSpeed)
{
pid_reentry = new PIDclamp("reentrySteer", 1, 0, 0, maxAngle);
reentryBurnSteerKp = kP;
reentryBurnMaxAoA = maxAngle;
reentryBurnAlt = alt;
reentryBurnTargetSpeed = tgtSpeed;
}
public void Reset()
{
pid1x = new PIDclamp("pid1x", kp1, ki1, kd1, vmax);
pid1y = new PIDclamp("pid1y", kp1y, ki1y, kd1y, vmax);
pid1z = new PIDclamp("pid1z", kp1, ki1, kd1, vmax);
// Scale up amax so its effectively ignored
pid2x = new PIDclamp("pid2x", kp2, ki2, kd2, amax);
pid2y = new PIDclamp("pid2y", kp2y, ki2y, kd2y, amax);
pid2z = new PIDclamp("pid2z", kp2, ki2, kd2, amax);
}
private void drawPIDValues(PIDclamp controller, string inputName, SASList controllerID)
{
stockPIDDisplay[(int)controllerID] = GUILayout.Toggle(stockPIDDisplay[(int)controllerID], inputName, GeneralUI.UISkin.customStyles[(int)myStyles.btnToggle]);
if (stockPIDDisplay[(int)controllerID])
{
controller.kp = GeneralUI.labPlusNumBox(GeneralUI.KpLabel, controller.kp.ToString(), 45);
controller.ki = GeneralUI.labPlusNumBox(GeneralUI.KiLabel, controller.ki.ToString(), 45);
controller.kd = GeneralUI.labPlusNumBox(GeneralUI.KdLabel, controller.kd.ToString(), 45);
controller.clamp = Math.Max(GeneralUI.labPlusNumBox(GeneralUI.ScalarLabel, controller.clamp.ToString(), 45), 0.01);
}
}
private void drawPIDValues(PIDclamp controller, string inputName, SASList controllerID)
{
stockPIDDisplay[(int)controllerID] = GUILayout.Toggle(stockPIDDisplay[(int)controllerID], inputName, GeneralUI.UISkin.customStyles[(int)myStyles.btnToggle]);
if (stockPIDDisplay[(int)controllerID])
{
controller.kp = GeneralUI.labPlusNumBox(GeneralUI.KpLabel, controller.kp.ToString(), 45);
controller.ki = GeneralUI.labPlusNumBox(GeneralUI.KiLabel, controller.ki.ToString(), 45);
controller.kd = GeneralUI.labPlusNumBox(GeneralUI.KdLabel, controller.kd.ToString(), 45);
controller.clamp = Math.Max(GeneralUI.labPlusNumBox(GeneralUI.ScalarLabel, controller.clamp.ToString(), 45), 0.01);
}
}
private void drawPIDValues(PIDclamp controller, string inputName, SASList controllerID)
{
stockPIDDisplay[(int)controllerID] = GUILayout.Toggle(stockPIDDisplay[(int)controllerID], inputName, GeneralUI.toggleButton);
if (stockPIDDisplay[(int)controllerID])
{
controller.kp = GeneralUI.labPlusNumBox("Kp:", controller.kp.ToString("G3"), 45);
controller.ki = GeneralUI.labPlusNumBox("Ki:", controller.ki.ToString("G3"), 45);
controller.kd = GeneralUI.labPlusNumBox("Kd:", controller.kd.ToString("G3"), 45);
controller.clamp = Math.Max(GeneralUI.labPlusNumBox("Scalar:", controller.clamp.ToString("G3"), 45), 0.01);
}
}
public void Update(VesselAutopilot.VesselSAS sas)
{
PIDclamp[] sasPID = new PIDclamp[3];
sasPID[(int)SASList.Pitch] = sas.pidLockedPitch;
sasPID[(int)SASList.Bank] = sas.pidLockedRoll;
sasPID[(int)SASList.Hdg] = sas.pidLockedYaw;
foreach (SASList s in Enum.GetValues(typeof(SASList)))
{
PIDGains[(int)s, 0] = sasPID[(int)s].kp;
PIDGains[(int)s, 1] = sasPID[(int)s].ki;
PIDGains[(int)s, 2] = sasPID[(int)s].kd;
PIDGains[(int)s, 3] = sasPID[(int)s].clamp;
}
}
public void Update(VesselAutopilot.VesselSAS sas)
{
PIDclamp[] sasPID = new PIDclamp[3];
sasPID[(int)SASList.Pitch] = sas.pidLockedPitch;
sasPID[(int)SASList.Roll] = sas.pidLockedRoll;
sasPID[(int)SASList.Yaw] = sas.pidLockedYaw;
foreach (SASList s in Enum.GetValues(typeof(SASList)))
{
PIDGains[(int)s, 0] = sasPID[(int)s].kp;
PIDGains[(int)s, 1] = sasPID[(int)s].ki;
PIDGains[(int)s, 2] = sasPID[(int)s].kd;
PIDGains[(int)s, 3] = sasPID[(int)s].clamp;
}
}
public void Init(float kp1, float ki1, float kd1, float kp2, float ki2, float kd2, float vmax, float amax, float ymult = 1.0f)
{
// Probably set I1=0, D1=0, I2=0, D2=0
// P1 determines proportion of position error to velocity 1, to close 1m as 1m/s
// P2 determines maximum spare acceleration available, 2+ times higher than P2 to avoid little oscillation
// I1>0 would help overcome something like a constant wind or gravity (but can compensate exactly for gravity)
// ymult scales up P1 for make the PID focus on hitting the height target
// (this is easier since we have gravity to pull us down if overshooting upward)
pid1x = new PIDclamp("pid1x", kp1, ki1, kd1, vmax);
pid1y = new PIDclamp("pid1y", kp1 * ymult, ki1, kd1, vmax);
pid1z = new PIDclamp("pid1z", kp1, ki1, kd1, vmax);
// Scale up amax so its effectively ignored
pid2x = new PIDclamp("pid2x", kp2, ki2, kd2, amax);
pid2y = new PIDclamp("pid2y", kp2 * ymult, ki2, kd2, amax);
pid2z = new PIDclamp("pid2z", kp2, ki2, kd2, amax);
}
public SASPreset(VesselAutopilot.VesselSAS sas, string Name) // used for adding a new stock preset
{
name = Name;
PIDclamp[] sasPID = new PIDclamp[3];
sasPID[(int)SASList.Pitch] = sas.pidLockedPitch;
sasPID[(int)SASList.Bank] = sas.pidLockedRoll;
sasPID[(int)SASList.Hdg] = sas.pidLockedYaw;
foreach (SASList s in Enum.GetValues(typeof(SASList)))
{
PIDGains[(int)s, 0] = sasPID[(int)s].kp;
PIDGains[(int)s, 1] = sasPID[(int)s].ki;
PIDGains[(int)s, 2] = sasPID[(int)s].kd;
PIDGains[(int)s, 3] = sasPID[(int)s].clamp;
}
}
public void InitLandingBurn(float kP, float maxAngle)
{
pid_landing = new PIDclamp("landingSteer", 1, 0, 0, maxAngle);
landingBurnSteerKp = kP;
landingBurnMaxAoA = maxAngle;
}
public void InitAeroDescent(float kP, float maxAngle)
{
pid_aero = new PIDclamp("aeroSteer", 1, 0, 0, maxAngle);
aeroDescentSteerKp = kP;
aeroDescentMaxAoA = maxAngle;
}
