diff --git a/src/kOS.Safe.Test/Structure/PIDLoopTest.cs b/src/kOS.Safe.Test/Structure/PIDLoopTest.cs new file mode 100644 index 0000000000..0e69953b96 --- /dev/null +++ b/src/kOS.Safe.Test/Structure/PIDLoopTest.cs @@ -0,0 +1,118 @@ +using System; +using kOS.Safe.Encapsulation; +using NUnit.Framework; + +namespace kOS.Safe.Test.Structure +{ + [TestFixture] + public class PIDLoopTest + { + + private double[] input; + + [SetUp] + public void Setup() + { + input = new double[] {-10, -10, -10, -10, 6, 6, 6, 6, 6, 6}; + } + + [Test] + public void CanCorrectlyReactToSineWaveInput() + { + var pidLoop = new PIDLoop(1, 1, 1); + double[] sineWaveInput = {0, -4.207, -4.547, -0.706, 3.784, 4.795, 1.397, -3.285, -4.947, -2.060}; + double[] output = new double[10]; + double[] pTermOutput = new double[10]; + double[] iTermOutput = new double[10]; + double[] dTermOutput = new double[10]; + + for (var i = 0; i < sineWaveInput.Length; i++) + { + output[i] = Math.Round((double) pidLoop.Update(i, sineWaveInput[i]), 2); + pTermOutput[i] = Math.Round(pidLoop.PTerm, 2); + iTermOutput[i] = Math.Round(pidLoop.ITerm, 2); + dTermOutput[i] = Math.Round(pidLoop.DTerm, 2); + } + + Assert.AreEqual(new double[] {0, 4.21, 4.55, 0.71, -3.78, -4.80, -1.40, 3.28, 4.95, 2.06}, pTermOutput, "Proportional part is incorrect"); + Assert.AreEqual(new double[] {0, 0, 4.21, 8.75, 9.46, 5.68, 0.88, -0.52, 2.77, 7.72}, iTermOutput, "Integral part is incorrect"); + Assert.AreEqual(new double[] {0, 4.21, 0.34, -3.84, -4.49, -1.01, 3.40, 4.68, 1.66, -2.89}, dTermOutput, "Derivative part is incorrect"); + Assert.AreEqual(new double[] {0, 8.41, 9.09, 5.62, 1.19, -0.13, 2.88, 7.45, 9.38, 6.89}, output, "PID output is incorrect"); + } + + [Test] + public void CanAntiWindUp() + { + var pidLoop = new PIDLoop(0, 1, 0, 11, 0); + double[] output = new double[10]; + + for (var i = 0; i < input.Length; i++) + { + output[i] = pidLoop.Update(i, input[i]); + } + + Assert.AreEqual(new double[] {0, 10, 11, 11, 11, 5, 0, 0, 0, 0}, output); + } + + [Test] + public void CanNoneAntiWindUp() + { + var pidLoop = new PIDLoop(0, 1, 0, 11, 0); + pidLoop.AntiWindupMode = "NONE"; + double[] outputs = new double[10]; + + for (var i = 0; i < input.Length; i++) + { + outputs[i] = pidLoop.Update(i, input[i]); + } + + Assert.AreEqual(new double[] {0, 10, 11, 11, 11, 11, 11, 11, 11, 10}, outputs); + } + + [Test] + public void CanClampingAntiWindUp() + { + var pidLoop = new PIDLoop(0, 1, 0, 11, 0); + pidLoop.AntiWindupMode = "CLAMPING"; + double[] outputs = new double[10]; + + for (var i = 0; i < input.Length; i++) + { + outputs[i] = pidLoop.Update(i, input[i]); + } + + Assert.AreEqual(new double[] {0, 10, 11, 11, 11, 11, 8, 2, 0, 0}, outputs); + } + + [Test] + public void CanBackCalculationAntiWindUp() + { + var pidLoop = new PIDLoop(0, 1, 0, 11, 0); + pidLoop.AntiWindupMode = "BACK-CALC"; + double[] outputs = new double[10]; + + for (var i = 0; i < input.Length; i++) + { + outputs[i] = pidLoop.Update(i, input[i]); + } + + Assert.AreEqual(new double[] {0, 10, 11, 11, 11, 5, 0, 0, 0, 0}, outputs); + } + + [Test] + public void CanBackCalculationWithK2AntiWindUp() + { + var pidLoop = new PIDLoop(0, 1, 0, 11, 0); + pidLoop.AntiWindupMode = "BACK-CALC"; + pidLoop.KBackCalc = 2; + double[] outputs = new double[10]; + + for (var i = 0; i < input.Length; i++) + { + outputs[i] = pidLoop.Update(i, input[i]); + } + + Assert.AreEqual(new double[] {0, 10, 11, 11, 11, 0, 0, 0, 0, 0}, outputs); + } + } +} diff --git a/src/kOS.Safe/Encapsulation/PIDLoop.cs b/src/kOS.Safe/Encapsulation/PIDLoop.cs index c2c36af9c9..814a004df6 100644 --- a/src/kOS.Safe/Encapsulation/PIDLoop.cs +++ b/src/kOS.Safe/Encapsulation/PIDLoop.cs @@ -62,8 +62,7 @@ public override void Execute(SafeSharedObjects shared) public static PIDLoop DeepCopy(PIDLoop source) { - PIDLoop newLoop = new PIDLoop - { + PIDLoop newLoop = new PIDLoop { LastSampleTime = source.LastSampleTime, Kp = source.Kp, Ki = source.Ki, @@ -74,12 +73,15 @@ public static PIDLoop DeepCopy(PIDLoop source) Output = source.Output, MinOutput = source.MinOutput, MaxOutput = source.MaxOutput, + AntiWindupMode = source.AntiWindupMode, + KBackCalc = source.KBackCalc, ErrorSum = source.ErrorSum, PTerm = source.PTerm, ITerm = source.ITerm, DTerm = source.DTerm, ExtraUnwind = source.ExtraUnwind, ChangeRate = source.ChangeRate, + lastIError = source.lastIError, unWinding = source.unWinding }; return newLoop; @@ -105,6 +107,10 @@ public static PIDLoop DeepCopy(PIDLoop source) public double MaxOutput { get; set; } + public string AntiWindupMode { get; set; } + + public double KBackCalc { get; set; } + public double Epsilon { get; set; } public double ErrorSum { get; set; } @@ -119,6 +125,7 @@ public static PIDLoop DeepCopy(PIDLoop source) public double ChangeRate { get; set; } + private double lastIError; private bool unWinding; public PIDLoop() @@ -138,6 +145,8 @@ public PIDLoop(double kp, double ki, double kd, double maxoutput = double.MaxVal Output = 0; MaxOutput = maxoutput; MinOutput = minoutput; + AntiWindupMode = "DEFAULT"; + KBackCalc = 1; Epsilon = nullzone; ErrorSum = 0; PTerm = 0; @@ -159,6 +168,8 @@ public void InitializeSuffixes() AddSuffix("OUTPUT", new Suffix(() => Output)); AddSuffix("MAXOUTPUT", new SetSuffix(() => MaxOutput, value => MaxOutput = value)); AddSuffix("MINOUTPUT", new SetSuffix(() => MinOutput, value => MinOutput = value)); + AddSuffix("ANTIWINDUPMODE", new SetSuffix(() => AntiWindupMode, value => AntiWindupMode = value)); + AddSuffix("KBACKCALC", new SetSuffix(() => KBackCalc, value => KBackCalc = value)); AddSuffix(new string[] { "IGNOREERROR", "EPSILON" }, new SetSuffix(() => Epsilon, value => Epsilon = value)); AddSuffix("ERRORSUM", new Suffix(() => ErrorSum)); AddSuffix("PTERM", new Suffix(() => PTerm)); @@ -185,6 +196,8 @@ public double Update(double sampleTime, double input, double setpoint, double ma public ScalarValue Update(ScalarValue sampleTime, ScalarValue input) { + if (LastSampleTime == sampleTime) return Output; + double error = Setpoint - input; if (error > -Epsilon && error < Epsilon) { @@ -192,72 +205,115 @@ public ScalarValue Update(ScalarValue sampleTime, ScalarValue input) // because the error is within the epsilon: error = 0; input = Setpoint; - Input = input; - Error = error; } - double pTerm = error * Kp; - double iTerm = 0; - double dTerm = 0; + + PTerm = error * Kp; + if (LastSampleTime < sampleTime) { double dt = sampleTime - LastSampleTime; + + ChangeRate = (error - Error) / dt; + DTerm = ChangeRate * Kd; + if (Ki != 0) { - if (ExtraUnwind) + ExtraUnwindIfEnabled(error); + ITerm += lastIError * dt; + } + else + { + ITerm = 0; + } + } + + lastIError = AntiWindup(error * Ki); + LastSampleTime = sampleTime; + Input = input; + Error = error; + if (Ki != 0) ErrorSum = ITerm / Ki; + else ErrorSum = 0; + + // Limit output according to MinOutput and MaxOutput + Output = Math.Min(MaxOutput, Math.Max(MinOutput, PTerm + ITerm + DTerm)); + return Output; + } + + private void ExtraUnwindIfEnabled(double error) + { + if (ExtraUnwind) + { + if (Math.Sign(error) != Math.Sign(ErrorSum)) + { + if (!unWinding) { - if (Math.Sign(error) != Math.Sign(ErrorSum)) - { - if (!unWinding) - { - Ki *= 2; - unWinding = true; - } - } - else if (unWinding) - { - Ki /= 2; - unWinding = false; - } + Ki *= 2; + unWinding = true; } - iTerm = ITerm + error * dt * Ki; } - ChangeRate = (input - Input) / dt; - if (Kd != 0) + else if (unWinding) { - dTerm = -ChangeRate * Kd; + Ki /= 2; + unWinding = false; } } - else + } + + private double AntiWindup(double iError) + { + double preSatOutput = PTerm + ITerm + DTerm; + + switch (AntiWindupMode.ToUpper()) { - dTerm = DTerm; - iTerm = ITerm; + case "NONE": + return iError; + case "CLAMPING": + return ClampingAntiWindup(preSatOutput, iError); + case "BACK-CALC": + return BackCalculationAntiWindup(preSatOutput, iError); + default: + return DefaultAntiWindup(preSatOutput, iError); } - Output = pTerm + iTerm + dTerm; - if (Output > MaxOutput) + } + + private double ClampingAntiWindup(double preSatOutput, double preIntError) + { + double preSatSign = 0; + if (preSatOutput > MaxOutput) { - Output = MaxOutput; - if (Ki != 0 && LastSampleTime < sampleTime) - { - iTerm = Output - Math.Min(pTerm + dTerm, MaxOutput); - } + preSatSign = 1; + } + else if (preSatOutput < MinOutput) + { + preSatSign = -1; } - if (Output < MinOutput) + + if (preSatSign != 0 && Math.Sign(preSatSign) == Math.Sign(preIntError)) { - Output = MinOutput; - if (Ki != 0 && LastSampleTime < sampleTime) - { - iTerm = Output - Math.Max(pTerm + dTerm, MinOutput); - } + preIntError = 0; } - LastSampleTime = sampleTime; - Input = input; - Error = error; - PTerm = pTerm; - ITerm = iTerm; - DTerm = dTerm; - if (Ki != 0) ErrorSum = iTerm / Ki; - else ErrorSum = 0; - return Output; + + return preIntError; + } + + private double BackCalculationAntiWindup(double preSatOutput, double preIntError) + { + double postSatOutput = Math.Min(MaxOutput, Math.Max(MinOutput, preSatOutput)); + return (postSatOutput - preSatOutput) * KBackCalc + preIntError; + } + + private double DefaultAntiWindup(double preSatOutput, double preIntError) + { + if (preSatOutput > MaxOutput) + { + ITerm = MaxOutput - Math.Min(PTerm + DTerm, MaxOutput); + } + if (preSatOutput < MinOutput) + { + ITerm = MinOutput - Math.Max(PTerm + DTerm, MinOutput); + } + + return preIntError; } public void ResetI() @@ -301,12 +357,14 @@ public override Dump Dump() result.Add("Setpoint", Setpoint); result.Add("MaxOutput", MaxOutput); result.Add("MinOutput", MinOutput); + result.Add("AntiWindupMode", AntiWindupMode); + result.Add("KBackCalc", KBackCalc); result.Add("ExtraUnwind", ExtraUnwind); return result; - } + } - public override void LoadDump(Dump dump) + public override void LoadDump(Dump dump) { Kp = Convert.ToDouble(dump["Kp"]); Ki = Convert.ToDouble(dump["Ki"]); @@ -314,7 +372,9 @@ public override void LoadDump(Dump dump) Setpoint = Convert.ToDouble(dump["Setpoint"]); MinOutput = Convert.ToDouble(dump["MinOutput"]); MaxOutput = Convert.ToDouble(dump["MaxOutput"]); + AntiWindupMode = Convert.ToString(dump["AntiWindupMode"]); + KBackCalc = Convert.ToDouble(dump["KBackCalc"]); ExtraUnwind = Convert.ToBoolean(dump["ExtraUnwind"]); } } -} \ No newline at end of file +}