PID Tuner RSAS patch

patch.diff

diff --git a/src/PIDTune/PIDTune.cs b/src/PIDTune/PIDTune.cs
index 179f3f1..6d620be 100644
--- a/src/PIDTune/PIDTune.cs
+++ b/src/PIDTune/PIDTune.cs
@@ -37,44 +37,77 @@ namespace regexKSP {
 		public Vector4 RollClamp;
 		[KSPField (isPersistant = true)]
 		public Vector4 YawClamp;
-		public Rect mainWindowPos = new Rect(Screen.width / 10, 20, 250, 130);
+		[KSPField(isPersistant = true)]
+		public Vector4 RPitchClamp;
+		[KSPField(isPersistant = true)]
+		public Vector4 RRollClamp;
+		[KSPField(isPersistant = true)]
+		public Vector4 RYawClamp;
+		public Rect mainWindowPos = new Rect(Screen.width / 10, 20, 250, 260);
 		private PIDInput pitch;
 		private PIDInput roll;
 		private PIDInput yaw;
+		private PIDInput Rpitch;
+		private PIDInput Rroll;
+		private PIDInput Ryaw;
 		private bool showGUI = false;
 		private bool loaded = false;
 
 		[KSPEvent (guiName = "Show PID Controls", guiActive = true, guiActiveEditor = false)]
 		public void showPIDControls() {
 			VesselAutopilot.VesselSAS sas = base.vessel.Autopilot.SAS;
-			pitch = new PIDInput(sas.pidLockedPitch.kp, sas.pidLockedPitch.ki, sas.pidLockedPitch.kd, sas.pidLockedPitch.clamp);
-			roll = new PIDInput(sas.pidLockedRoll.kp, sas.pidLockedRoll.ki, sas.pidLockedRoll.kd, sas.pidLockedRoll.clamp);
-			yaw = new PIDInput(sas.pidLockedYaw.kp, sas.pidLockedYaw.ki, sas.pidLockedYaw.kd, sas.pidLockedYaw.clamp);
+			VesselAutopilot.VesselRSAS Rsas = base.vessel.Autopilot.RSAS;
+			pitch = new PIDInput((float)sas.pidLockedPitch.kp, (float)sas.pidLockedPitch.ki, (float)sas.pidLockedPitch.kd, (float)sas.pidLockedPitch.clamp);
+			roll = new PIDInput((float)sas.pidLockedRoll.kp, (float)sas.pidLockedRoll.ki, (float)sas.pidLockedRoll.kd, (float)sas.pidLockedRoll.clamp);
+			yaw = new PIDInput((float)sas.pidLockedYaw.kp, (float)sas.pidLockedYaw.ki, (float)sas.pidLockedYaw.kd, (float)sas.pidLockedYaw.clamp);
+			Rpitch = new PIDInput(Rsas.pidPitch.KP, Rsas.pidPitch.KI, Rsas.pidPitch.KD, 1);
+			Rroll = new PIDInput(Rsas.pidRoll.KP, Rsas.pidRoll.KI, Rsas.pidRoll.KD, 1);
+			Ryaw = new PIDInput(Rsas.pidYaw.KP, Rsas.pidYaw.KI, Rsas.pidYaw.KD, 1);
 			showGUI = true;
 		}
 
 		public override void OnFixedUpdate() {
 			if(!loaded) {
-				if(PitchClamp.SqrMagnitude() != 0f) {
+				if(PitchClamp != Vector4.zero) {
 					base.vessel.Autopilot.SAS.SetLockPitchPID(PitchClamp.x, PitchClamp.y, PitchClamp.z, PitchClamp.w);
 				}
-				if(RollClamp.SqrMagnitude() != 0f) {
+				if(RollClamp != Vector4.zero) {
 					base.vessel.Autopilot.SAS.SetLockRollPID(RollClamp.x, RollClamp.y, RollClamp.z, RollClamp.w);
 				}
-				if(YawClamp.SqrMagnitude() != 0f) {
+				if(YawClamp != Vector4.zero) {
 					base.vessel.Autopilot.SAS.SetLockYawPID(YawClamp.x, YawClamp.y, YawClamp.z, YawClamp.w);
 				}
+				if (RPitchClamp != Vector4.zero)
+				{
+					base.vessel.Autopilot.RSAS.pidPitch.ReinitializePIDsOnly(RPitchClamp.x / RPitchClamp.w, RPitchClamp.y / RPitchClamp.w, RPitchClamp.z / RPitchClamp.w);
+				}
+                if (RRollClamp != Vector4.zero)
+				{
+                    base.vessel.Autopilot.RSAS.pidRoll.ReinitializePIDsOnly(RRollClamp.x / RRollClamp.w, RRollClamp.y / RRollClamp.w, RRollClamp.z / RRollClamp.w);
+				}
+                if (RYawClamp != Vector4.zero)
+				{
+                    base.vessel.Autopilot.RSAS.pidYaw.ReinitializePIDsOnly(RYawClamp.x / RYawClamp.w, RYawClamp.y / RYawClamp.w, RYawClamp.z / RYawClamp.w);
+				}
 				loaded = true;
 			}
 		}
 
-		public void CommitChanges(PIDclamp pitch, PIDclamp roll, PIDclamp yaw) {
+		public void CommitChanges(PIDclamp pitch, PIDclamp roll, PIDclamp yaw, PIDclamp Rpitch, PIDclamp Rroll, PIDclamp Ryaw)
+		{
 			PitchClamp = new Vector4((float)pitch.kp, (float)pitch.ki, (float)pitch.kd, (float)pitch.clamp);
 			RollClamp = new Vector4((float)roll.kp, (float)roll.ki, (float)roll.kd, (float)roll.clamp);
 			YawClamp = new Vector4((float)yaw.kp, (float)yaw.ki, (float)yaw.kd, (float)yaw.clamp);
 			base.vessel.Autopilot.SAS.SetLockPitchPID((float)pitch.kp, (float)pitch.ki, (float)pitch.kd, (float)pitch.clamp);
 			base.vessel.Autopilot.SAS.SetLockRollPID((float)roll.kp, (float)roll.ki, (float)roll.kd, (float)roll.clamp);
 			base.vessel.Autopilot.SAS.SetLockYawPID((float)yaw.kp, (float)yaw.ki, (float)yaw.kd, (float)yaw.clamp);
+			
+			RPitchClamp = new Vector4((float)Rpitch.kp, (float)Rpitch.ki, (float)Rpitch.kd, (float)Rpitch.clamp);
+			RRollClamp = new Vector4((float)Rroll.kp, (float)Rroll.ki, (float)Rroll.kd, (float)Rroll.clamp);
+			RYawClamp = new Vector4((float)Ryaw.kp, (float)Ryaw.ki, (float)Ryaw.kd, (float)Ryaw.clamp);
+			base.vessel.Autopilot.RSAS.pidPitch.ReinitializePIDsOnly((float)Rpitch.kp / (float)Rpitch.clamp, (float)Rpitch.ki / (float)Rpitch.clamp, (float)Rpitch.kd / (float)Rpitch.clamp);
+			base.vessel.Autopilot.RSAS.pidRoll.ReinitializePIDsOnly((float)Rroll.kp / (float)Rroll.clamp, (float)Rroll.ki / (float)Rroll.clamp, (float)Rroll.kd / (float)Rroll.clamp);
+			base.vessel.Autopilot.RSAS.pidYaw.ReinitializePIDsOnly((float)Ryaw.kp / (float)Ryaw.clamp, (float)Ryaw.ki / (float)Ryaw.clamp, (float)Ryaw.kd / (float)Ryaw.clamp);
 		}
 
 		public void OnGUI() {
@@ -90,9 +123,9 @@ namespace regexKSP {
 
 			GUILayout.BeginVertical();
 
-// pitch controls
+			// pitch controls
 			GUILayout.BeginHorizontal();
-			GUILayout.Label("Pitch");
+			GUILayout.Label("Stability Assist Pitch");
 			GUILayout.EndHorizontal();
 
 			GUILayout.BeginHorizontal();
@@ -144,10 +177,10 @@ namespace regexKSP {
 
 
 			GUILayout.BeginHorizontal();
-			GUILayout.Label("Roll");
+			GUILayout.Label("Stability Assist Roll");
 			GUILayout.EndHorizontal();
 
-// roll controls
+			// roll controls
 			GUILayout.BeginHorizontal();
 			GUILayout.Label("kp", GUILayout.Width(50));
 			if(!roll.kp_parsed) {
@@ -195,9 +228,9 @@ namespace regexKSP {
 
 			GUILayout.EndHorizontal();
 
-// yaw controls
+			// yaw controls
 			GUILayout.BeginHorizontal();
-			GUILayout.Label("Yaw");
+			GUILayout.Label("Stability Assist Yaw");
 			GUILayout.EndHorizontal();
 
 			GUILayout.BeginHorizontal();
@@ -247,18 +280,206 @@ namespace regexKSP {
 
 			GUILayout.EndHorizontal();
 
+			// Rpitch controls
+			GUILayout.BeginHorizontal();
+			GUILayout.Label("Relative Pitch");
+			GUILayout.EndHorizontal();
+
+			GUILayout.BeginHorizontal();
+			GUILayout.Label("kp", GUILayout.Width(50));
+			if (!Rpitch.kp_parsed)
+			{
+				GUI.contentColor = Color.red;
+			}
+			stmp = GUILayout.TextField(Rpitch.kp, GUILayout.Width(50));
+			if (!Rpitch.kp.Equals(stmp, StringComparison.Ordinal))
+			{
+				Rpitch.kp_parsed = checkParseString(stmp);
+				Rpitch.kp = stmp;
+			}
+			GUI.contentColor = contentColor;
+
+			GUILayout.Label("ki", GUILayout.Width(50));
+			if (!Rpitch.ki_parsed)
+			{
+				GUI.contentColor = Color.red;
+			}
+			stmp = GUILayout.TextField(Rpitch.ki, GUILayout.Width(50));
+			if (!Rpitch.ki.Equals(stmp, StringComparison.Ordinal))
+			{
+				Rpitch.ki_parsed = checkParseString(stmp);
+				Rpitch.ki = stmp;
+			}
+			GUI.contentColor = contentColor;
+
+			GUILayout.Label("kd", GUILayout.Width(50));
+			if (!Rpitch.kd_parsed)
+			{
+				GUI.contentColor = Color.red;
+			}
+			stmp = GUILayout.TextField(Rpitch.kd, GUILayout.Width(50));
+			if (!Rpitch.kd.Equals(stmp, StringComparison.Ordinal))
+			{
+				Rpitch.kd_parsed = checkParseString(stmp);
+				Rpitch.kd = stmp;
+			}
+			GUI.contentColor = contentColor;
+
+			GUILayout.Label("clamp", GUILayout.Width(50));
+			if (!Rpitch.clamp_parsed)
+			{
+				GUI.contentColor = Color.red;
+			}
+			stmp = GUILayout.TextField(Rpitch.clamp, GUILayout.Width(50));
+			if (!Rpitch.clamp.Equals(stmp, StringComparison.Ordinal))
+			{
+				Rpitch.clamp_parsed = checkParseString(stmp);
+				Rpitch.clamp = stmp;
+			}
+			GUI.contentColor = contentColor;
+
+			GUILayout.EndHorizontal();
+
+
+			GUILayout.BeginHorizontal();
+			GUILayout.Label("Relative Roll");
+			GUILayout.EndHorizontal();
+
+			// roll controls
 			GUILayout.BeginHorizontal();
-			if(GUILayout.Button("Cancel")) {
+			GUILayout.Label("kp", GUILayout.Width(50));
+			if (!Rroll.kp_parsed)
+			{
+				GUI.contentColor = Color.red;
+			}
+			stmp = GUILayout.TextField(Rroll.kp, GUILayout.Width(50));
+			if (!Rroll.kp.Equals(stmp, StringComparison.Ordinal))
+			{
+				Rroll.kp_parsed = checkParseString(stmp);
+				Rroll.kp = stmp;
+			}
+			GUI.contentColor = contentColor;
+
+			GUILayout.Label("ki", GUILayout.Width(50));
+			if (!Rroll.ki_parsed)
+			{
+				GUI.contentColor = Color.red;
+			}
+			stmp = GUILayout.TextField(Rroll.ki, GUILayout.Width(50));
+			if (!Rroll.ki.Equals(stmp, StringComparison.Ordinal))
+			{
+				Rroll.ki_parsed = checkParseString(stmp);
+				Rroll.ki = stmp;
+			}
+			GUI.contentColor = contentColor;
+
+			GUILayout.Label("kd", GUILayout.Width(50));
+			if (!Rroll.kd_parsed)
+			{
+				GUI.contentColor = Color.red;
+			}
+			stmp = GUILayout.TextField(Rroll.kd, GUILayout.Width(50));
+			if (!Rroll.kd.Equals(stmp, StringComparison.Ordinal))
+			{
+				Rroll.kd_parsed = checkParseString(stmp);
+				Rroll.kd = stmp;
+			}
+			GUI.contentColor = contentColor;
+
+			GUILayout.Label("clamp", GUILayout.Width(50));
+			if (!Rroll.clamp_parsed)
+			{
+				GUI.contentColor = Color.red;
+			}
+			stmp = GUILayout.TextField(Rroll.clamp, GUILayout.Width(50));
+			if (!Rroll.clamp.Equals(stmp, StringComparison.Ordinal))
+			{
+				Rroll.clamp_parsed = checkParseString(stmp);
+				Rroll.clamp = stmp;
+			}
+			GUI.contentColor = contentColor;
+
+			GUILayout.EndHorizontal();
+
+			// yaw controls
+			GUILayout.BeginHorizontal();
+			GUILayout.Label("Relative Yaw");
+			GUILayout.EndHorizontal();
+
+			GUILayout.BeginHorizontal();
+			GUILayout.Label("kp", GUILayout.Width(50));
+			if (!Ryaw.kp_parsed)
+			{
+				GUI.contentColor = Color.red;
+			}
+			stmp = GUILayout.TextField(Ryaw.kp, GUILayout.Width(50));
+			if (!Ryaw.kp.Equals(stmp, StringComparison.Ordinal))
+			{
+				Ryaw.kp_parsed = checkParseString(stmp);
+				Ryaw.kp = stmp;
+			}
+			GUI.contentColor = contentColor;
+
+			GUILayout.Label("ki", GUILayout.Width(50));
+			if (!Ryaw.ki_parsed)
+			{
+				GUI.contentColor = Color.red;
+			}
+			stmp = GUILayout.TextField(Ryaw.ki, GUILayout.Width(50));
+			if (!Ryaw.ki.Equals(stmp, StringComparison.Ordinal))
+			{
+				Ryaw.ki_parsed = checkParseString(stmp);
+				Ryaw.ki = stmp;
+			}
+			GUI.contentColor = contentColor;
+
+			GUILayout.Label("kd", GUILayout.Width(50));
+			if (!Ryaw.kd_parsed)
+			{
+				GUI.contentColor = Color.red;
+			}
+			stmp = GUILayout.TextField(Ryaw.kd, GUILayout.Width(50));
+			if (!Ryaw.kd.Equals(stmp, StringComparison.Ordinal))
+			{
+				Ryaw.kd_parsed = checkParseString(stmp);
+				Ryaw.kd = stmp;
+			}
+			GUI.contentColor = contentColor;
+
+			GUILayout.Label("clamp", GUILayout.Width(50));
+			if (!Ryaw.clamp_parsed)
+			{
+				GUI.contentColor = Color.red;
+			}
+			stmp = GUILayout.TextField(Ryaw.clamp, GUILayout.Width(50));
+			if (!Ryaw.clamp.Equals(stmp, StringComparison.Ordinal))
+			{
+				Ryaw.clamp_parsed = checkParseString(stmp);
+				Ryaw.clamp = stmp;
+			}
+			GUI.contentColor = contentColor;
+
+			GUILayout.EndHorizontal();
+
+			// buttons
+			GUILayout.BeginHorizontal();
+			if (GUILayout.Button("Cancel"))
+			{
 				showGUI = false;
 			}
-			if(GUILayout.Button("Commit")) {
-				if(pitch.proper() && roll.proper() && yaw.proper()) {
-					CommitChanges(pitch.buildPIDclamp(), roll.buildPIDclamp(), yaw.buildPIDclamp());
+
+			if (GUILayout.Button("Commit"))
+			{
+				if (pitch.proper() && roll.proper() && yaw.proper() && Rpitch.proper() && Rroll.proper() && Ryaw.proper())
+				{
+					CommitChanges(pitch.buildPIDclamp(), roll.buildPIDclamp(), yaw.buildPIDclamp(), Rpitch.buildPIDclamp(), Rroll.buildPIDclamp(), Ryaw.buildPIDclamp());
 				}
 			}
-			if(GUILayout.Button("Save and Close")) {
-				if(pitch.proper() && roll.proper() && yaw.proper()) {
-					CommitChanges(pitch.buildPIDclamp(), roll.buildPIDclamp(), yaw.buildPIDclamp());
+			if (GUILayout.Button("Save and Close"))
+			{
+				if ((pitch.proper() && roll.proper() && yaw.proper() && Rpitch.proper() && Rroll.proper() && Ryaw.proper()))
+				{
+					CommitChanges(pitch.buildPIDclamp(), roll.buildPIDclamp(), yaw.buildPIDclamp(), Rpitch.buildPIDclamp(), Rroll.buildPIDclamp(), Ryaw.buildPIDclamp());
 					showGUI = false;
 				}
 			}
@@ -289,7 +510,7 @@ namespace regexKSP {
 			public string clamp;
 			public bool clamp_parsed = true;
 
-			public PIDInput(double kp, double ki, double kd, double clamp) {
+			public PIDInput(float kp, float ki, float kd, float clamp) {
 				this.kp = kp.ToString();
 				this.ki = ki.ToString();
 				this.kd = kd.ToString();