From 83d036aa2709910f1a3b7da2e11cd0c922c3499e Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Fri, 14 Jun 2013 19:26:35 -0700 Subject: [PATCH 01/35] Stabilize spaceplane at high altitude Spaceplanes at high altitude develop an altitude oscillation. This should smooth out flights in kerbin atmosphere. --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index aa9ac41e3..9c6dfa5cd 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -81,7 +81,7 @@ public override void Drive(FlightCtrlState s) public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { - double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / 30.0; + double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * ((69000 / 2) / vesselState.altitudeASL)); double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 9), (float)Math.Sin(Math.PI / 9))); AimVelocityVector(targetFlightPathAngle, targetHeading); } From 18d256e85cb2e855dcae04cb017951f791715cce Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Fri, 14 Jun 2013 19:46:11 -0700 Subject: [PATCH 02/35] Spaceplane high altitude stability Spaceplanes develop an altitude oscillation at high altitude. This should slow down the ascent pattern at high altitudes to stabilize higher altitude flight paths. --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 9c6dfa5cd..aa9ac41e3 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -81,7 +81,7 @@ public override void Drive(FlightCtrlState s) public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { - double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * ((69000 / 2) / vesselState.altitudeASL)); + double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / 30.0; double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 9), (float)Math.Sin(Math.PI / 9))); AimVelocityVector(targetFlightPathAngle, targetHeading); } From bf1564cf12459c224789a421fc8963e04c26e846 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Fri, 14 Jun 2013 19:51:59 -0700 Subject: [PATCH 03/35] Revert "Spaceplane high altitude stability" This reverts commit 18d256e85cb2e855dcae04cb017951f791715cce. --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index aa9ac41e3..9c6dfa5cd 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -81,7 +81,7 @@ public override void Drive(FlightCtrlState s) public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { - double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / 30.0; + double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * ((69000 / 2) / vesselState.altitudeASL)); double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 9), (float)Math.Sin(Math.PI / 9))); AimVelocityVector(targetFlightPathAngle, targetHeading); } From c6e5a9ac1cecfd8af04d34208847b62918177878 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Fri, 14 Jun 2013 20:06:02 -0700 Subject: [PATCH 04/35] Spaceplane high altitude stability Spaceplanes develop an altitude oscillation at high altitude. This should slow down the ascent pattern at high altitudes to stabilize higher altitude flight paths. --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 9c6dfa5cd..73c4489e9 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -81,7 +81,7 @@ public override void Drive(FlightCtrlState s) public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { - double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * ((69000 / 2) / vesselState.altitudeASL)); + double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow(((69000 - vesselState.altitudeASL) / 69000), 2)); double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 9), (float)Math.Sin(Math.PI / 9))); AimVelocityVector(targetFlightPathAngle, targetHeading); } From f1b925a52ac22faff96ab5abe4080870d46343e4 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Fri, 14 Jun 2013 20:07:25 -0700 Subject: [PATCH 05/35] Spaceplane Fix --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 73c4489e9..bd780ce5f 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -81,7 +81,7 @@ public override void Drive(FlightCtrlState s) public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { - double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow(((69000 - vesselState.altitudeASL) / 69000), 2)); + double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((69000 / (69000 - vesselState.altitudeASL)), 2)); double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 9), (float)Math.Sin(Math.PI / 9))); AimVelocityVector(targetFlightPathAngle, targetHeading); } From bf9ec09e85c6f337071f914efecea56f8bec38a6 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Fri, 14 Jun 2013 21:06:39 -0700 Subject: [PATCH 06/35] Allows spaceplane to compensate on all celestial bodies --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index bd780ce5f..f18d76089 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -81,7 +81,7 @@ public override void Drive(FlightCtrlState s) public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { - double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((69000 / (69000 - vesselState.altitudeASL)), 2)); + double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((RealMaxAtmosphereAltitude(mainBody) / (RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 2)); double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 9), (float)Math.Sin(Math.PI / 9))); AimVelocityVector(targetFlightPathAngle, targetHeading); } From 32bf3eee2670598c9a38579e58127585474aa159 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Fri, 14 Jun 2013 23:49:29 -0700 Subject: [PATCH 07/35] Fixed to actually work --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index f18d76089..d2a0b56d0 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -81,7 +81,7 @@ public override void Drive(FlightCtrlState s) public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { - double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((RealMaxAtmosphereAltitude(mainBody) / (RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 2)); + double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) / (CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 2)); double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 9), (float)Math.Sin(Math.PI / 9))); AimVelocityVector(targetFlightPathAngle, targetHeading); } From cfdf28ac28a526004678fcf80eeb09266fd142ea Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Sat, 15 Jun 2013 13:20:57 -0700 Subject: [PATCH 08/35] Target climb rate change --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index d2a0b56d0..b084ef3c3 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -81,7 +81,7 @@ public override void Drive(FlightCtrlState s) public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { - double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) / (CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 2)); + double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (45.0 * Math.Pow((CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) / (CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 2)); double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 9), (float)Math.Sin(Math.PI / 9))); AimVelocityVector(targetFlightPathAngle, targetHeading); } From 1f4a4a44b4d66f6d21c067a98f9da460045f4ff5 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Sun, 16 Jun 2013 19:00:51 -0700 Subject: [PATCH 09/35] Out of the atmosphere Changes to stabilize spaceplanes up to the upper edges of the atmosphere through the transition into space. --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index b084ef3c3..286e6d19f 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -81,8 +81,8 @@ public override void Drive(FlightCtrlState s) public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { - double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (45.0 * Math.Pow((CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) / (CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 2)); - double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 9), (float)Math.Sin(Math.PI / 9))); + double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) / (CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 4)); + double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 6), (float)Math.Sin(Math.PI / 6))); AimVelocityVector(targetFlightPathAngle, targetHeading); } @@ -124,6 +124,7 @@ public void DriveAutoland(FlightCtrlState s) public float maxYaw = 10.0F; public float maxRoll = 10.0F; public float maxPitchCorrection = 5.0F; + public float maxPitchDiff = 25.0F; public double AoAtimeConstant = 2.0; public double pitchCorrectionTimeConstant = 15.0; @@ -137,7 +138,8 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) double noseRoll = (maxRoll / maxYaw) * headingTurn; //vertical control - double nosePitch = desiredFpa + stableAoA + pitchCorrection; + double vesselDriectionPitch = Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI); + double nosePitch = Mathf.Clamp((float)(desiredFpa + stableAoA + pitchCorrection), (float)(vesselDriectionPitch), (float)(vesselDriectionPitch + maxPitchDiff)); core.attitude.attitudeTo(noseHeading, nosePitch, noseRoll, this); From 10b8cb143f22ed6b5cb0f881c7eb4b205e11acce Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Thu, 20 Jun 2013 19:22:13 -0700 Subject: [PATCH 10/35] Switching over to PID controllers. Replaced the old flight path controller code with 2 PID controllers (Pitch and Heading). Added more code to make takeoff and landing smoother and make sure the gear is down whenever it is coming close to landing (eg. in a manual landing scenario). --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 90 +++++++++++++++----- 1 file changed, 71 insertions(+), 19 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 286e6d19f..e90c9087e 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -14,6 +14,7 @@ public void Autoland(object controller) core.attitude.users.Add(this); mode = Mode.AUTOLAND; loweredGear = false; + brakes = false; } public void HoldHeadingAndAltitude(object controller) @@ -59,6 +60,7 @@ public enum Mode { AUTOLAND, HOLD, OFF }; public EditableDouble targetHeading = 90; bool loweredGear = false; + bool brakes = false; public override void OnModuleDisabled() { @@ -79,22 +81,67 @@ public override void Drive(FlightCtrlState s) } } + bool gearDown = false; public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { + if (!part.vessel.Landed) + { + if (!gearDown) + { + if (part.vessel.terrainAltitude < 100.0) + { + vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, true); + gearDown = true; + } + } + else + { + if (part.vessel.terrainAltitude > 100.0) + { + vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, false); + gearDown = false; + } + } + + if (landed) + { + vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, false); + maxRoll = 30.0F; + maxYaw = 3.0F; + landed = false; + } + } + else + { + if (!landed) + { + maxRoll = 1.0F; + maxYaw = 2.0F; + landed = true; + } + } + double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) / (CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 4)); double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 6), (float)Math.Sin(Math.PI / 6))); AimVelocityVector(targetFlightPathAngle, targetHeading); } + bool landed = false; + double landTime = 0; public void DriveAutoland(FlightCtrlState s) { if (!part.vessel.Landed) { + if (landed) landed = false; + Vector3d runwayStart = RunwayStart(); + //prepare for landing if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 1000.0) { vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, true); + maxRoll = 1.0F; + maxYaw = 2.0F; loweredGear = true; } @@ -111,6 +158,18 @@ public void DriveAutoland(FlightCtrlState s) } else { + if (!landed) + { + landTime = vesselState.time; + landed = true; + } + //apply breaks a little after touchdown + if (!brakes && vesselState.time > landTime + 1.0) + { + vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true); + vessel.ctrlState.mainThrottle = 0; + brakes = true; + } //keep the plane aligned with the runway: Vector3d runwayDir = runway.End(vesselState.CoM) - runway.Start(vesselState.CoM); if (Vector3d.Dot(runwayDir, vesselState.forward) < 0) runwayDir *= -1; @@ -119,36 +178,29 @@ public void DriveAutoland(FlightCtrlState s) } } - public double stableAoA = 0; //we average AoA over time to get an estimate of what pitch will produce what FPA - public double pitchCorrection = 0; //we average (commanded pitch - actual pitch) over time in order to fix this offset in our commands - public float maxYaw = 10.0F; - public float maxRoll = 10.0F; - public float maxPitchCorrection = 5.0F; - public float maxPitchDiff = 25.0F; - public double AoAtimeConstant = 2.0; - public double pitchCorrectionTimeConstant = 15.0; - + public float maxYaw = 3.0F; + public float maxRoll = 30.0F; + public PIDController velocityPID = new PIDController(1, 0.01, 0.5); + public PIDController headingPID = new PIDController(0.2, 0.0, 0.6); + public float maxPitchCorrection = 15.0F; + public float maxPitchDiffUp = 25.0F; + public float maxPitchDiffDown = 10.0F; void AimVelocityVector(double desiredFpa, double desiredHeading) { //horizontal control double velocityHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.east), Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.north)); - double headingTurn = Mathf.Clamp((float)MuUtils.ClampDegrees180(desiredHeading - velocityHeading), -maxYaw, maxYaw); + //double headingTurn = Mathf.Clamp((float)MuUtils.ClampDegrees180(desiredHeading - velocityHeading), -maxYaw, maxYaw); + double headingTurn = Mathf.Clamp((float)MuUtils.ClampDegrees180(headingPID.Compute(desiredHeading - velocityHeading)), -maxYaw, maxYaw); double noseHeading = velocityHeading + headingTurn; double noseRoll = (maxRoll / maxYaw) * headingTurn; //vertical control - double vesselDriectionPitch = Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI); - double nosePitch = Mathf.Clamp((float)(desiredFpa + stableAoA + pitchCorrection), (float)(vesselDriectionPitch), (float)(vesselDriectionPitch + maxPitchDiff)); + double velocityPitch = Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI); + double pitchCorrection = Mathf.Clamp((float)velocityPID.Compute(desiredFpa - velocityPitch), -maxPitchCorrection, maxPitchCorrection); + double nosePitch = Mathf.Clamp((float)(desiredFpa + pitchCorrection), (float)(velocityPitch - maxPitchDiffDown), (float)(velocityPitch + maxPitchDiffUp)); core.attitude.attitudeTo(noseHeading, nosePitch, noseRoll, this); - - double flightPathAngle = 180 / Math.PI * Math.Atan2(vesselState.speedVertical, vesselState.speedSurfaceHorizontal); - double AoA = vesselState.vesselPitch - flightPathAngle; - stableAoA = (AoAtimeConstant * stableAoA + vesselState.deltaT * AoA) / (AoAtimeConstant + vesselState.deltaT); //a sort of integral error - - pitchCorrection = (pitchCorrectionTimeConstant * pitchCorrection + vesselState.deltaT * (nosePitch - vesselState.vesselPitch)) / (pitchCorrectionTimeConstant + vesselState.deltaT); - pitchCorrection = Mathf.Clamp((float)pitchCorrection, -maxPitchCorrection, maxPitchCorrection); } Vector3d RunwayStart() From b865b39d716057583e224dcc8b001be54281d93e Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Fri, 21 Jun 2013 00:45:00 -0700 Subject: [PATCH 11/35] Fixes and PID tweaks --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 24 +++++++++++++++----- MechJeb2/PIDController.cs | 7 ++++-- 2 files changed, 23 insertions(+), 8 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index e90c9087e..3a9068402 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -13,6 +13,7 @@ public void Autoland(object controller) users.Add(controller); core.attitude.users.Add(this); mode = Mode.AUTOLAND; + landed = false; loweredGear = false; brakes = false; } @@ -22,6 +23,9 @@ public void HoldHeadingAndAltitude(object controller) users.Add(controller); core.attitude.users.Add(this); mode = Mode.HOLD; + landed = true; + loweredGear = false; + brakes = false; } public void AutopilotOff() @@ -64,6 +68,7 @@ public enum Mode { AUTOLAND, HOLD, OFF }; public override void OnModuleDisabled() { + core.attitude.attitudeDeactivate(); } @@ -115,6 +120,7 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { if (!landed) { + vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true); maxRoll = 1.0F; maxYaw = 2.0F; landed = true; @@ -136,6 +142,12 @@ public void DriveAutoland(FlightCtrlState s) Vector3d runwayStart = RunwayStart(); + //fine tune approach + if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 3000.0) + { + maxRoll = 5.0F; + maxYaw = 5.0F; + } //prepare for landing if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 1000.0) { @@ -152,7 +164,7 @@ public void DriveAutoland(FlightCtrlState s) double verticalDistanceToRunway = Vector3d.Dot(vectorToRunway, vesselState.up); double horizontalDistanceToRunway = Math.Sqrt(vectorToRunway.sqrMagnitude - verticalDistanceToRunway * verticalDistanceToRunway); double flightPathAngleToRunway = 180 / Math.PI * Math.Atan2(verticalDistanceToRunway, horizontalDistanceToRunway); - double desiredFPA = Mathf.Clamp((float)(flightPathAngleToRunway + 3 * (flightPathAngleToRunway + glideslope)), -20.0F, 0.0F); + double desiredFPA = Mathf.Clamp((float)(flightPathAngleToRunway + 3 * (flightPathAngleToRunway + glideslope)), -20.0F, 5.0F); AimVelocityVector(desiredFPA, headingToWaypoint); } @@ -160,6 +172,7 @@ public void DriveAutoland(FlightCtrlState s) { if (!landed) { + vessel.ctrlState.mainThrottle = 0; landTime = vesselState.time; landed = true; } @@ -167,7 +180,6 @@ public void DriveAutoland(FlightCtrlState s) if (!brakes && vesselState.time > landTime + 1.0) { vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true); - vessel.ctrlState.mainThrottle = 0; brakes = true; } //keep the plane aligned with the runway: @@ -180,8 +192,8 @@ public void DriveAutoland(FlightCtrlState s) public float maxYaw = 3.0F; public float maxRoll = 30.0F; - public PIDController velocityPID = new PIDController(1, 0.01, 0.5); - public PIDController headingPID = new PIDController(0.2, 0.0, 0.6); + public PIDController pitchPID = new PIDController(1, 0.01, 0.5, 50, -50, 0.96); + public PIDController headingPID = new PIDController(0.2, 0, 0.6); public float maxPitchCorrection = 15.0F; public float maxPitchDiffUp = 25.0F; public float maxPitchDiffDown = 10.0F; @@ -191,13 +203,13 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) double velocityHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.east), Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.north)); //double headingTurn = Mathf.Clamp((float)MuUtils.ClampDegrees180(desiredHeading - velocityHeading), -maxYaw, maxYaw); - double headingTurn = Mathf.Clamp((float)MuUtils.ClampDegrees180(headingPID.Compute(desiredHeading - velocityHeading)), -maxYaw, maxYaw); + double headingTurn = Mathf.Clamp((float)headingPID.Compute(MuUtils.ClampDegrees180(desiredHeading - velocityHeading)), -maxYaw, maxYaw); double noseHeading = velocityHeading + headingTurn; double noseRoll = (maxRoll / maxYaw) * headingTurn; //vertical control double velocityPitch = Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI); - double pitchCorrection = Mathf.Clamp((float)velocityPID.Compute(desiredFpa - velocityPitch), -maxPitchCorrection, maxPitchCorrection); + double pitchCorrection = Mathf.Clamp((float)pitchPID.Compute(desiredFpa - velocityPitch), -maxPitchCorrection, maxPitchCorrection); double nosePitch = Mathf.Clamp((float)(desiredFpa + pitchCorrection), (float)(velocityPitch - maxPitchDiffDown), (float)(velocityPitch + maxPitchDiffUp)); core.attitude.attitudeTo(noseHeading, nosePitch, noseRoll, this); diff --git a/MechJeb2/PIDController.cs b/MechJeb2/PIDController.cs index 6d12fba15..a6df17b6b 100644 --- a/MechJeb2/PIDController.cs +++ b/MechJeb2/PIDController.cs @@ -8,21 +8,24 @@ namespace MuMech { public class PIDController : IConfigNode { - public double prevError, intAccum, Kp, Ki, Kd, max, min; + public double prevError, intAccum, intDecay, Kp, Ki, Kd, max, min; - public PIDController(double Kp = 0, double Ki = 0, double Kd = 0, double max = double.MaxValue, double min = double.MinValue) + public PIDController(double Kp = 0, double Ki = 0, double Kd = 0, double max = double.MaxValue, double min = double.MinValue, double intDecay = 0) { this.Kp = Kp; this.Ki = Ki; this.Kd = Kd; this.max = max; this.min = min; + this.intDecay = intDecay; Reset(); } public double Compute(double error) { + if (intDecay != 0) intAccum *= intDecay; intAccum += error * TimeWarp.fixedDeltaTime; + if (intDecay != 0) intAccum /= intDecay; //for larger decays double action = (Kp * error) + (Ki * intAccum) + (Kd * (error - prevError) / TimeWarp.fixedDeltaTime); double clamped = Math.Max(min, Math.Min(max, action)); if (clamped != action) From a6936512ea9cb3e13e81ffba037e9e55f0046e35 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Sat, 22 Jun 2013 00:33:39 -0700 Subject: [PATCH 12/35] Takeoff/Landing refinement Changed how the spaceplane approaches the runway for landing and the yaw/roll parameters through decent and landing. Also undid the changes made to the PID controller and refined the PID values. --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 77 +++++++++++++++----- MechJeb2/MechJebModuleSpaceplaneGuidance.cs | 12 +++ MechJeb2/PIDController.cs | 5 +- 3 files changed, 73 insertions(+), 21 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 3a9068402..890453ec4 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -23,7 +23,7 @@ public void HoldHeadingAndAltitude(object controller) users.Add(controller); core.attitude.users.Add(this); mode = Mode.HOLD; - landed = true; + landed = false; loweredGear = false; brakes = false; } @@ -108,6 +108,7 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) } } + //takeoff or set for flight if (landed) { vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, false); @@ -120,11 +121,24 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { if (!landed) { - vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true); + vessel.ctrlState.mainThrottle = 0; maxRoll = 1.0F; maxYaw = 2.0F; + landTime = vesselState.time; landed = true; } + //apply breaks a little after touchdown + if (!brakes && vesselState.time > landTime + 1.0) + { + vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true); + brakes = true; + } + //if engines are started, disengage brakes and prepare for takeoff + if (brakes && vessel.ctrlState.mainThrottle > 0.01) + { + vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, false); + core.attitude.attitudeKILLROT = true; + } } double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) / (CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 4)); @@ -134,6 +148,8 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) bool landed = false; double landTime = 0; + public double aimAltitude = 0; + public double distanceFrom = 0; public void DriveAutoland(FlightCtrlState s) { if (!part.vessel.Landed) @@ -143,17 +159,22 @@ public void DriveAutoland(FlightCtrlState s) Vector3d runwayStart = RunwayStart(); //fine tune approach + if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 9000.0) + { + maxRoll = 8.0F; + maxYaw = 3.0F; + } if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 3000.0) { maxRoll = 5.0F; - maxYaw = 5.0F; + maxYaw = 2.0F; } //prepare for landing if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 1000.0) { vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, true); - maxRoll = 1.0F; - maxYaw = 2.0F; + maxRoll = 3.0F; + maxYaw = 1.0F; loweredGear = true; } @@ -163,10 +184,25 @@ public void DriveAutoland(FlightCtrlState s) Vector3d vectorToRunway = runwayStart - vesselState.CoM; double verticalDistanceToRunway = Vector3d.Dot(vectorToRunway, vesselState.up); double horizontalDistanceToRunway = Math.Sqrt(vectorToRunway.sqrMagnitude - verticalDistanceToRunway * verticalDistanceToRunway); - double flightPathAngleToRunway = 180 / Math.PI * Math.Atan2(verticalDistanceToRunway, horizontalDistanceToRunway); - double desiredFPA = Mathf.Clamp((float)(flightPathAngleToRunway + 3 * (flightPathAngleToRunway + glideslope)), -20.0F, 5.0F); + distanceFrom = horizontalDistanceToRunway; + if (horizontalDistanceToRunway > 10000) + { + aimAltitude = 1500; + double horizontalDifference = 10000; + if (horizontalDistanceToRunway > 30000) { aimAltitude = 3000; horizontalDifference = 30000; } + if (horizontalDistanceToRunway > 50000) { aimAltitude+= 8000; horizontalDifference = 50000; } + double setupFPA = 180 / Math.PI * Math.Atan2(aimAltitude - vessel.altitude, horizontalDistanceToRunway - horizontalDifference); + + AimVelocityVector(setupFPA, headingToWaypoint); + } + else + { + double flightPathAngleToRunway = 180 / Math.PI * Math.Atan2(verticalDistanceToRunway, horizontalDistanceToRunway); + double desiredFPA = Mathf.Clamp((float)(flightPathAngleToRunway + 3 * (flightPathAngleToRunway + glideslope)), -20.0F, 5.0F); - AimVelocityVector(desiredFPA, headingToWaypoint); + aimAltitude = verticalDistanceToRunway; + AimVelocityVector(desiredFPA, headingToWaypoint); + } } else { @@ -174,6 +210,8 @@ public void DriveAutoland(FlightCtrlState s) { vessel.ctrlState.mainThrottle = 0; landTime = vesselState.time; + maxRoll = 1.0F; + maxYaw = 2.0F; landed = true; } //apply breaks a little after touchdown @@ -192,11 +230,11 @@ public void DriveAutoland(FlightCtrlState s) public float maxYaw = 3.0F; public float maxRoll = 30.0F; - public PIDController pitchPID = new PIDController(1, 0.01, 0.5, 50, -50, 0.96); - public PIDController headingPID = new PIDController(0.2, 0, 0.6); - public float maxPitchCorrection = 15.0F; - public float maxPitchDiffUp = 25.0F; - public float maxPitchDiffDown = 10.0F; + public PIDController pitchCorrectionPID = new PIDController(3, 1, 0.5, 30, -10); + public PIDController headingPID = new PIDController(1, 0, 0.5); + public double nosePitch = 0; + public double noseHeading = 0; + public double noseRoll = 0; void AimVelocityVector(double desiredFpa, double desiredHeading) { //horizontal control @@ -204,15 +242,20 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.north)); //double headingTurn = Mathf.Clamp((float)MuUtils.ClampDegrees180(desiredHeading - velocityHeading), -maxYaw, maxYaw); double headingTurn = Mathf.Clamp((float)headingPID.Compute(MuUtils.ClampDegrees180(desiredHeading - velocityHeading)), -maxYaw, maxYaw); - double noseHeading = velocityHeading + headingTurn; - double noseRoll = (maxRoll / maxYaw) * headingTurn; + noseHeading = velocityHeading + headingTurn; + noseRoll = (maxRoll / maxYaw) * headingTurn; //vertical control double velocityPitch = Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI); - double pitchCorrection = Mathf.Clamp((float)pitchPID.Compute(desiredFpa - velocityPitch), -maxPitchCorrection, maxPitchCorrection); - double nosePitch = Mathf.Clamp((float)(desiredFpa + pitchCorrection), (float)(velocityPitch - maxPitchDiffDown), (float)(velocityPitch + maxPitchDiffUp)); + double pitchCorrection = pitchCorrectionPID.Compute(desiredFpa - velocityPitch); + nosePitch = Mathf.Clamp((float)(desiredFpa + pitchCorrection), (float)(velocityPitch + pitchCorrectionPID.min), (float)(velocityPitch + pitchCorrectionPID.max)); + + if (landed && vesselState.velocityMainBodySurface.magnitude < 50) nosePitch = vesselState.vesselPitch; + //core.attitude.attitudeKILLROT = true; core.attitude.attitudeTo(noseHeading, nosePitch, noseRoll, this); + + if (pitchCorrectionPID.intAccum > 50) pitchCorrectionPID.intAccum = 50; } Vector3d RunwayStart() diff --git a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs index 8f583dfd6..784f083dd 100644 --- a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs +++ b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs @@ -56,6 +56,18 @@ protected override void WindowGUI(int windowID) GUILayout.Label("m"); GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); + GUILayout.Label("pitch PID:"); + autopilot.pitchCorrectionPID.Kp = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPID.Kp.ToString(), GUILayout.Width(40))); + autopilot.pitchCorrectionPID.Ki = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPID.Ki.ToString(), GUILayout.Width(40))); + autopilot.pitchCorrectionPID.Kd = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPID.Kd.ToString(), GUILayout.Width(40))); + GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); + GUILayout.Label("heading PD:"); + autopilot.headingPID.Kp = double.Parse(GUILayout.TextField(autopilot.headingPID.Kp.ToString(), GUILayout.Width(40))); + autopilot.headingPID.Kd = double.Parse(GUILayout.TextField(autopilot.headingPID.Kd.ToString(), GUILayout.Width(40))); + GUILayout.EndHorizontal(); + if (autopilot.enabled && autopilot.mode == MechJebModuleSpaceplaneAutopilot.Mode.HOLD && GUILayout.Button("Abort")) autopilot.AutopilotOff(); diff --git a/MechJeb2/PIDController.cs b/MechJeb2/PIDController.cs index a6df17b6b..764d3dc15 100644 --- a/MechJeb2/PIDController.cs +++ b/MechJeb2/PIDController.cs @@ -10,22 +10,19 @@ public class PIDController : IConfigNode { public double prevError, intAccum, intDecay, Kp, Ki, Kd, max, min; - public PIDController(double Kp = 0, double Ki = 0, double Kd = 0, double max = double.MaxValue, double min = double.MinValue, double intDecay = 0) + public PIDController(double Kp = 0, double Ki = 0, double Kd = 0, double max = double.MaxValue, double min = double.MinValue) { this.Kp = Kp; this.Ki = Ki; this.Kd = Kd; this.max = max; this.min = min; - this.intDecay = intDecay; Reset(); } public double Compute(double error) { - if (intDecay != 0) intAccum *= intDecay; intAccum += error * TimeWarp.fixedDeltaTime; - if (intDecay != 0) intAccum /= intDecay; //for larger decays double action = (Kp * error) + (Ki * intAccum) + (Kd * (error - prevError) / TimeWarp.fixedDeltaTime); double clamped = Math.Max(min, Math.Min(max, action)); if (clamped != action) From dbd0da51a73605b41a11c7e69d5320fbae8f13a0 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Sat, 22 Jun 2013 00:36:00 -0700 Subject: [PATCH 13/35] Final undo to the PID controller --- MechJeb2/PIDController.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MechJeb2/PIDController.cs b/MechJeb2/PIDController.cs index 764d3dc15..6d12fba15 100644 --- a/MechJeb2/PIDController.cs +++ b/MechJeb2/PIDController.cs @@ -8,7 +8,7 @@ namespace MuMech { public class PIDController : IConfigNode { - public double prevError, intAccum, intDecay, Kp, Ki, Kd, max, min; + public double prevError, intAccum, Kp, Ki, Kd, max, min; public PIDController(double Kp = 0, double Ki = 0, double Kd = 0, double max = double.MaxValue, double min = double.MinValue) { From 34924eb843eb294babb1d932b9e71c3484b68658 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Sat, 22 Jun 2013 01:14:24 -0700 Subject: [PATCH 14/35] Removed minimum velocity for takeoff --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 2 -- 1 file changed, 2 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 890453ec4..687e37a8a 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -250,8 +250,6 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) double pitchCorrection = pitchCorrectionPID.Compute(desiredFpa - velocityPitch); nosePitch = Mathf.Clamp((float)(desiredFpa + pitchCorrection), (float)(velocityPitch + pitchCorrectionPID.min), (float)(velocityPitch + pitchCorrectionPID.max)); - if (landed && vesselState.velocityMainBodySurface.magnitude < 50) nosePitch = vesselState.vesselPitch; - //core.attitude.attitudeKILLROT = true; core.attitude.attitudeTo(noseHeading, nosePitch, noseRoll, this); From a06cb8365c3fdeef35a6771987c3500859d93a80 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Sun, 23 Jun 2013 21:34:55 -0700 Subject: [PATCH 15/35] Changes to takeoff and landing Implements possible permanent fixes for takeoff and landing. --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 88 ++++++++++++++------ MechJeb2/MechJebModuleSpaceplaneGuidance.cs | 10 ++- 2 files changed, 69 insertions(+), 29 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 687e37a8a..ef6d00eea 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -8,10 +8,12 @@ namespace MuMech { public class MechJebModuleSpaceplaneAutopilot : ComputerModule { + object controller; + public void Autoland(object controller) { users.Add(controller); - core.attitude.users.Add(this); + AutopilotOn(); mode = Mode.AUTOLAND; landed = false; loweredGear = false; @@ -21,18 +23,31 @@ public void Autoland(object controller) public void HoldHeadingAndAltitude(object controller) { users.Add(controller); - core.attitude.users.Add(this); + AutopilotOn(); mode = Mode.HOLD; landed = false; loweredGear = false; brakes = false; } + bool autopilotOn = false; + public void AutopilotOn() + { + core.attitude.users.Add(this); + autopilotOn = true; + } + public void AutopilotOff() { - mode = Mode.OFF; - users.Clear(); core.attitude.attitudeDeactivate(); + autopilotOn = false; + } + + public void Abort() + { + users.Clear(); + AutopilotOff(); + mode = Mode.OFF; } public static Runway[] runways = @@ -87,10 +102,13 @@ public override void Drive(FlightCtrlState s) } bool gearDown = false; + public float takeoffPitch = 0.5F; public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { if (!part.vessel.Landed) { + if (!autopilotOn) + AutopilotOn(); if (!gearDown) { if (part.vessel.terrainAltitude < 100.0) @@ -131,14 +149,21 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) if (!brakes && vesselState.time > landTime + 1.0) { vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true); + AutopilotOff(); brakes = true; } //if engines are started, disengage brakes and prepare for takeoff if (brakes && vessel.ctrlState.mainThrottle > 0.01) { vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, false); - core.attitude.attitudeKILLROT = true; + brakes = false; + } + //pitch up for takeoff + if (!brakes && vesselState.time > landTime + 1.0) + { + vessel.ctrlState.pitch = takeoffPitch; } + vessel.ctrlState.yaw = (float)(targetHeading - vesselState.vesselHeading) * 0.1F; } double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) / (CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 4)); @@ -150,6 +175,7 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) double landTime = 0; public double aimAltitude = 0; public double distanceFrom = 0; + public double runwayHeading = 0; public void DriveAutoland(FlightCtrlState s) { if (!part.vessel.Landed) @@ -158,6 +184,9 @@ public void DriveAutoland(FlightCtrlState s) Vector3d runwayStart = RunwayStart(); + if (!autopilotOn) + AutopilotOn(); + //fine tune approach if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 9000.0) { @@ -190,7 +219,7 @@ public void DriveAutoland(FlightCtrlState s) aimAltitude = 1500; double horizontalDifference = 10000; if (horizontalDistanceToRunway > 30000) { aimAltitude = 3000; horizontalDifference = 30000; } - if (horizontalDistanceToRunway > 50000) { aimAltitude+= 8000; horizontalDifference = 50000; } + if (horizontalDistanceToRunway > 50000) { aimAltitude = 8000; horizontalDifference = 50000; } double setupFPA = 180 / Math.PI * Math.Atan2(aimAltitude - vessel.altitude, horizontalDistanceToRunway - horizontalDifference); AimVelocityVector(setupFPA, headingToWaypoint); @@ -210,21 +239,24 @@ public void DriveAutoland(FlightCtrlState s) { vessel.ctrlState.mainThrottle = 0; landTime = vesselState.time; - maxRoll = 1.0F; - maxYaw = 2.0F; + AutopilotOff(); + //maxRoll = 1.0F; + //maxYaw = 2.0F; landed = true; } //apply breaks a little after touchdown if (!brakes && vesselState.time > landTime + 1.0) { vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true); + vessel.ctrlState.pitch = -1; brakes = true; } //keep the plane aligned with the runway: Vector3d runwayDir = runway.End(vesselState.CoM) - runway.Start(vesselState.CoM); if (Vector3d.Dot(runwayDir, vesselState.forward) < 0) runwayDir *= -1; - double runwayHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(runwayDir, vesselState.east), Vector3d.Dot(runwayDir, vesselState.north)); - core.attitude.attitudeTo(runwayHeading, 0, 0, this); + runwayHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(runwayDir, vesselState.east), Vector3d.Dot(runwayDir, vesselState.north)); + //core.attitude.attitudeTo(runwayHeading, 0, 0, this); + vessel.ctrlState.yaw = (float)MuUtils.ClampDegrees360(runwayHeading - vesselState.vesselHeading) * 0.1F; } } @@ -237,23 +269,25 @@ public void DriveAutoland(FlightCtrlState s) public double noseRoll = 0; void AimVelocityVector(double desiredFpa, double desiredHeading) { - //horizontal control - double velocityHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.east), - Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.north)); - //double headingTurn = Mathf.Clamp((float)MuUtils.ClampDegrees180(desiredHeading - velocityHeading), -maxYaw, maxYaw); - double headingTurn = Mathf.Clamp((float)headingPID.Compute(MuUtils.ClampDegrees180(desiredHeading - velocityHeading)), -maxYaw, maxYaw); - noseHeading = velocityHeading + headingTurn; - noseRoll = (maxRoll / maxYaw) * headingTurn; - - //vertical control - double velocityPitch = Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI); - double pitchCorrection = pitchCorrectionPID.Compute(desiredFpa - velocityPitch); - nosePitch = Mathf.Clamp((float)(desiredFpa + pitchCorrection), (float)(velocityPitch + pitchCorrectionPID.min), (float)(velocityPitch + pitchCorrectionPID.max)); - - //core.attitude.attitudeKILLROT = true; - core.attitude.attitudeTo(noseHeading, nosePitch, noseRoll, this); - - if (pitchCorrectionPID.intAccum > 50) pitchCorrectionPID.intAccum = 50; + if (autopilotOn) + { + //horizontal control + double velocityHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.east), + Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.north)); + //double headingTurn = Mathf.Clamp((float)MuUtils.ClampDegrees180(desiredHeading - velocityHeading), -maxYaw, maxYaw); + double headingTurn = Mathf.Clamp((float)headingPID.Compute(MuUtils.ClampDegrees180(desiredHeading - velocityHeading)), -maxYaw, maxYaw); + noseHeading = velocityHeading + headingTurn; + noseRoll = (maxRoll / maxYaw) * headingTurn; + + //vertical control + double velocityPitch = Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI); + double pitchCorrection = pitchCorrectionPID.Compute(desiredFpa - velocityPitch); + nosePitch = Mathf.Clamp((float)(desiredFpa + pitchCorrection), (float)(velocityPitch + pitchCorrectionPID.min), (float)(velocityPitch + pitchCorrectionPID.max)); + + core.attitude.attitudeTo(noseHeading, nosePitch, noseRoll, this); + + if (pitchCorrectionPID.intAccum > 50) pitchCorrectionPID.intAccum = 50; + } } Vector3d RunwayStart() diff --git a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs index 784f083dd..07cbfc5d4 100644 --- a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs +++ b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs @@ -41,7 +41,7 @@ protected override void WindowGUI(int windowID) if (GUILayout.Button("Autoland")) autopilot.Autoland(this); if (autopilot.enabled && autopilot.mode == MechJebModuleSpaceplaneAutopilot.Mode.AUTOLAND - && GUILayout.Button("Abort")) autopilot.AutopilotOff(); + && GUILayout.Button("Abort")) autopilot.Abort(); GuiUtils.SimpleTextBox("Autoland glideslope:", autopilot.glideslope, "º"); @@ -56,6 +56,12 @@ protected override void WindowGUI(int windowID) GUILayout.Label("m"); GUILayout.EndHorizontal(); + //temp + GUILayout.BeginHorizontal(); + GUILayout.Label("Takeoff Pitch:"); + autopilot.takeoffPitch = float.Parse(GUILayout.TextField(autopilot.takeoffPitch.ToString(), GUILayout.Width(40))); + GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); GUILayout.Label("pitch PID:"); autopilot.pitchCorrectionPID.Kp = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPID.Kp.ToString(), GUILayout.Width(40))); @@ -69,7 +75,7 @@ protected override void WindowGUI(int windowID) GUILayout.EndHorizontal(); if (autopilot.enabled && autopilot.mode == MechJebModuleSpaceplaneAutopilot.Mode.HOLD - && GUILayout.Button("Abort")) autopilot.AutopilotOff(); + && GUILayout.Button("Abort")) autopilot.Abort(); GUILayout.EndVertical(); From b15e528ff91c9fae0a04cd2034b17f7541ffce65 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Sun, 23 Jun 2013 23:27:57 -0700 Subject: [PATCH 16/35] Takeoff and landing adjustments --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 4 ++-- MechJeb2/MechJebModuleSpaceplaneGuidance.cs | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index ef6d00eea..6197d8f24 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -163,7 +163,7 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { vessel.ctrlState.pitch = takeoffPitch; } - vessel.ctrlState.yaw = (float)(targetHeading - vesselState.vesselHeading) * 0.1F; + vessel.ctrlState.yaw = (float)MuUtils.ClampDegrees180(targetHeading - vesselState.vesselHeading) * 0.1F; } double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) / (CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 4)); @@ -256,7 +256,7 @@ public void DriveAutoland(FlightCtrlState s) if (Vector3d.Dot(runwayDir, vesselState.forward) < 0) runwayDir *= -1; runwayHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(runwayDir, vesselState.east), Vector3d.Dot(runwayDir, vesselState.north)); //core.attitude.attitudeTo(runwayHeading, 0, 0, this); - vessel.ctrlState.yaw = (float)MuUtils.ClampDegrees360(runwayHeading - vesselState.vesselHeading) * 0.1F; + vessel.ctrlState.yaw = (float)MuUtils.ClampDegrees180(runwayHeading - vesselState.vesselHeading) * 0.1F; } } diff --git a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs index 07cbfc5d4..55c3d5085 100644 --- a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs +++ b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs @@ -68,6 +68,7 @@ protected override void WindowGUI(int windowID) autopilot.pitchCorrectionPID.Ki = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPID.Ki.ToString(), GUILayout.Width(40))); autopilot.pitchCorrectionPID.Kd = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPID.Kd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); GUILayout.Label("heading PD:"); autopilot.headingPID.Kp = double.Parse(GUILayout.TextField(autopilot.headingPID.Kp.ToString(), GUILayout.Width(40))); From 0df44b3631b907392d8783b28522b5be0b4bf8f8 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Mon, 24 Jun 2013 00:34:08 -0700 Subject: [PATCH 17/35] Controller removed from stored variable --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 2 -- 1 file changed, 2 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 6197d8f24..f77911f54 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -8,8 +8,6 @@ namespace MuMech { public class MechJebModuleSpaceplaneAutopilot : ComputerModule { - object controller; - public void Autoland(object controller) { users.Add(controller); From 74fc5c3c049118f682174983c7d1c2920c9d5891 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Mon, 24 Jun 2013 20:58:01 -0700 Subject: [PATCH 18/35] Added a low pass filter for possible future use --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 65 ++++++++++++++++---- 1 file changed, 53 insertions(+), 12 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index f77911f54..a9dfb6e71 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -12,6 +12,8 @@ public void Autoland(object controller) { users.Add(controller); AutopilotOn(); + maxRoll = 22.5F; + maxYaw = 0.5F; mode = Mode.AUTOLAND; landed = false; loweredGear = false; @@ -22,6 +24,8 @@ public void HoldHeadingAndAltitude(object controller) { users.Add(controller); AutopilotOn(); + maxRoll = 22.5F; + maxYaw = 0.5F; mode = Mode.HOLD; landed = false; loweredGear = false; @@ -128,8 +132,8 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) if (landed) { vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, false); - maxRoll = 30.0F; - maxYaw = 3.0F; + maxRoll = 22.5F; + maxYaw = 0.5F; landed = false; } } @@ -138,8 +142,7 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) if (!landed) { vessel.ctrlState.mainThrottle = 0; - maxRoll = 1.0F; - maxYaw = 2.0F; + AutopilotOff(); landTime = vesselState.time; landed = true; } @@ -147,7 +150,6 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) if (!brakes && vesselState.time > landTime + 1.0) { vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true); - AutopilotOff(); brakes = true; } //if engines are started, disengage brakes and prepare for takeoff @@ -189,19 +191,19 @@ public void DriveAutoland(FlightCtrlState s) if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 9000.0) { maxRoll = 8.0F; - maxYaw = 3.0F; + maxYaw = 0.5F; } if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 3000.0) { maxRoll = 5.0F; - maxYaw = 2.0F; + maxYaw = 0.5F; } //prepare for landing if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 1000.0) { vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, true); maxRoll = 3.0F; - maxYaw = 1.0F; + maxYaw = 0.5F; loweredGear = true; } @@ -238,8 +240,6 @@ public void DriveAutoland(FlightCtrlState s) vessel.ctrlState.mainThrottle = 0; landTime = vesselState.time; AutopilotOff(); - //maxRoll = 1.0F; - //maxYaw = 2.0F; landed = true; } //apply breaks a little after touchdown @@ -258,8 +258,8 @@ public void DriveAutoland(FlightCtrlState s) } } - public float maxYaw = 3.0F; - public float maxRoll = 30.0F; + public float maxYaw = 0.5F; + public float maxRoll = 22.5F; public PIDController pitchCorrectionPID = new PIDController(3, 1, 0.5, 30, -10); public PIDController headingPID = new PIDController(1, 0, 0.5); public double nosePitch = 0; @@ -275,6 +275,7 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) //double headingTurn = Mathf.Clamp((float)MuUtils.ClampDegrees180(desiredHeading - velocityHeading), -maxYaw, maxYaw); double headingTurn = Mathf.Clamp((float)headingPID.Compute(MuUtils.ClampDegrees180(desiredHeading - velocityHeading)), -maxYaw, maxYaw); noseHeading = velocityHeading + headingTurn; + double roll = Mathf.Clamp(Mathf.Abs((float)headingTurn * 3), 3, maxRoll); noseRoll = (maxRoll / maxYaw) * headingTurn; //vertical control @@ -288,6 +289,46 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) } } + /* + public class LowPass360 + { + public double y_old, alpha, timeConstant, dt; + + public LowPass360(double timeConst) + { + this.timeConstant = timeConst; + clear(); + initialize(); + } + + private void initialize() + { + dt = TimeWarp.fixedDeltaTime; + this.alpha = TimeWarp.fixedDeltaTime / (timeConstant - TimeWarp.fixedDeltaTime); + } + + public double calc(double x) + { + if (dt != TimeWarp.fixedDeltaTime) + { + initialize(); + y_old = x; + } + else + { + y_old = MuUtils.ClampDegrees360(y_old + alpha * MuUtils.ClampDegrees180(x - y_old)); + } + + return y_old; + } + + public void clear() + { + y_old = 0; + } + } + */ + Vector3d RunwayStart() { Vector3d runwayStart = runway.Start(vesselState.CoM); From 64be04ee5af95e2e50b3e202a29d07a1607dd7db Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Tue, 25 Jun 2013 17:08:31 -0700 Subject: [PATCH 19/35] Replaced ascent guidance with PID controllers Removed the ascent guidance controller and replaced it with dedicated PID controllers for pitch and roll. To smooth out the surface control movements, a low pass filter was implemented. A throttle control was also added to help with ascent and landing. --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 112 ++++++++++++------- MechJeb2/MechJebModuleSpaceplaneGuidance.cs | 32 +++++- 2 files changed, 101 insertions(+), 43 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index a9dfb6e71..d5e4a908e 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -13,7 +13,6 @@ public void Autoland(object controller) users.Add(controller); AutopilotOn(); maxRoll = 22.5F; - maxYaw = 0.5F; mode = Mode.AUTOLAND; landed = false; loweredGear = false; @@ -25,7 +24,6 @@ public void HoldHeadingAndAltitude(object controller) users.Add(controller); AutopilotOn(); maxRoll = 22.5F; - maxYaw = 0.5F; mode = Mode.HOLD; landed = false; loweredGear = false; @@ -35,13 +33,11 @@ public void HoldHeadingAndAltitude(object controller) bool autopilotOn = false; public void AutopilotOn() { - core.attitude.users.Add(this); autopilotOn = true; } public void AutopilotOff() { - core.attitude.attitudeDeactivate(); autopilotOn = false; } @@ -132,8 +128,7 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) if (landed) { vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, false); - maxRoll = 22.5F; - maxYaw = 0.5F; + maxRoll = 25.0F; landed = false; } } @@ -187,23 +182,10 @@ public void DriveAutoland(FlightCtrlState s) if (!autopilotOn) AutopilotOn(); - //fine tune approach - if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 9000.0) - { - maxRoll = 8.0F; - maxYaw = 0.5F; - } - if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 3000.0) - { - maxRoll = 5.0F; - maxYaw = 0.5F; - } //prepare for landing if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 1000.0) { vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, true); - maxRoll = 3.0F; - maxYaw = 0.5F; loweredGear = true; } @@ -253,18 +235,24 @@ public void DriveAutoland(FlightCtrlState s) Vector3d runwayDir = runway.End(vesselState.CoM) - runway.Start(vesselState.CoM); if (Vector3d.Dot(runwayDir, vesselState.forward) < 0) runwayDir *= -1; runwayHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(runwayDir, vesselState.east), Vector3d.Dot(runwayDir, vesselState.north)); - //core.attitude.attitudeTo(runwayHeading, 0, 0, this); + vessel.ctrlState.pitch = -0.5F; vessel.ctrlState.yaw = (float)MuUtils.ClampDegrees180(runwayHeading - vesselState.vesselHeading) * 0.1F; } } - public float maxYaw = 0.5F; - public float maxRoll = 22.5F; - public PIDController pitchCorrectionPID = new PIDController(3, 1, 0.5, 30, -10); - public PIDController headingPID = new PIDController(1, 0, 0.5); - public double nosePitch = 0; - public double noseHeading = 0; + public float maxRoll = 25.0F; + public PIDController pitchPID = new PIDController(1, 0.3, 0.1, 25, -10); + public PIDController pitchCorrectionPID = new PIDController(0.2, 0.05, 0.1, 1, -1); + public PIDController rollPID = new PIDController(2.5, 0, 0.5); + public PIDController rollCorrectionPID = new PIDController(0.006, 0, 0.003, 1, -1); + public LowPass180 rollLowPass = new LowPass180(0.35); public double noseRoll = 0; + public double desiredAoA = 0; + public double pitchCorrection = 0; + public double desiredRoll = 0; + public double rollCorrection = 0; + public float throttleUpPitch = 0.5F; + public float throttleDownPitch = 0.0F; void AimVelocityVector(double desiredFpa, double desiredHeading) { if (autopilotOn) @@ -272,24 +260,33 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) //horizontal control double velocityHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.east), Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.north)); - //double headingTurn = Mathf.Clamp((float)MuUtils.ClampDegrees180(desiredHeading - velocityHeading), -maxYaw, maxYaw); - double headingTurn = Mathf.Clamp((float)headingPID.Compute(MuUtils.ClampDegrees180(desiredHeading - velocityHeading)), -maxYaw, maxYaw); - noseHeading = velocityHeading + headingTurn; - double roll = Mathf.Clamp(Mathf.Abs((float)headingTurn * 3), 3, maxRoll); - noseRoll = (maxRoll / maxYaw) * headingTurn; + noseRoll = rollLowPass.calc(vesselState.vesselRoll); + desiredRoll = -Mathf.Clamp((float)rollPID.Compute(MuUtils.ClampDegrees180(desiredHeading - velocityHeading)), -maxRoll, maxRoll); + rollCorrection = Mathf.Clamp((float)rollCorrectionPID.Compute(noseRoll - desiredRoll), (float)rollCorrectionPID.min, (float)rollCorrectionPID.max); //vertical control double velocityPitch = Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI); - double pitchCorrection = pitchCorrectionPID.Compute(desiredFpa - velocityPitch); - nosePitch = Mathf.Clamp((float)(desiredFpa + pitchCorrection), (float)(velocityPitch + pitchCorrectionPID.min), (float)(velocityPitch + pitchCorrectionPID.max)); - - core.attitude.attitudeTo(noseHeading, nosePitch, noseRoll, this); - - if (pitchCorrectionPID.intAccum > 50) pitchCorrectionPID.intAccum = 50; + desiredAoA = Mathf.Clamp((float)pitchPID.Compute(desiredFpa - velocityPitch), (float)pitchPID.min, (float)pitchPID.max); + pitchCorrection = Mathf.Clamp((float)pitchCorrectionPID.Compute(desiredAoA - (vesselState.vesselPitch - velocityPitch)), (float)pitchCorrectionPID.min, (float)pitchCorrectionPID.max); + + vessel.ctrlState.roll = (float)rollCorrection; + vessel.ctrlState.pitch = (float)pitchCorrection; + + if (pitchPID.intAccum > 100) pitchPID.intAccum = 100; + if (pitchPID.intAccum < -100) pitchPID.intAccum = -100; + if (pitchCorrectionPID.intAccum > 10) pitchCorrectionPID.intAccum = 10; + if (pitchCorrectionPID.intAccum < -10) pitchCorrectionPID.intAccum = -10; + //if (rollPID.intAccum > 100) rollPID.intAccum = 100; + //if (rollPID.intAccum < -100) rollPID.intAccum = -100; + //if (rollCorrectionPID.intAccum > 10) rollCorrectionPID.intAccum = 10; + //if (rollCorrectionPID.intAccum < -10) rollCorrectionPID.intAccum = -10; + + //regulate throttle + if (pitchCorrection > throttleUpPitch) vessel.ctrlState.mainThrottle += 0.002F * (float)(pitchCorrection - throttleUpPitch) * vessel.ctrlState.mainThrottle; + if (pitchCorrection < throttleDownPitch) vessel.ctrlState.mainThrottle += 0.002F * (float)(pitchCorrection - throttleDownPitch) * vessel.ctrlState.mainThrottle; } } - /* public class LowPass360 { public double y_old, alpha, timeConstant, dt; @@ -327,7 +324,44 @@ public void clear() y_old = 0; } } - */ + + public class LowPass180 + { + public double y_old, alpha, timeConstant, dt; + + public LowPass180(double timeConst) + { + this.timeConstant = timeConst; + clear(); + initialize(); + } + + private void initialize() + { + dt = TimeWarp.fixedDeltaTime; + this.alpha = TimeWarp.fixedDeltaTime / (timeConstant - TimeWarp.fixedDeltaTime); + } + + public double calc(double x) + { + if (dt != TimeWarp.fixedDeltaTime) + { + initialize(); + y_old = x; + } + else + { + y_old = MuUtils.ClampDegrees180(y_old + alpha * MuUtils.ClampDegrees180(x - y_old)); + } + + return y_old; + } + + public void clear() + { + y_old = 0; + } + } Vector3d RunwayStart() { diff --git a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs index 55c3d5085..27d08c13e 100644 --- a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs +++ b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs @@ -56,24 +56,48 @@ protected override void WindowGUI(int windowID) GUILayout.Label("m"); GUILayout.EndHorizontal(); - //temp GUILayout.BeginHorizontal(); GUILayout.Label("Takeoff Pitch:"); autopilot.takeoffPitch = float.Parse(GUILayout.TextField(autopilot.takeoffPitch.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); + //temp + //GUILayout.Label(autopilot.desiredAoA.ToString()); + //GUILayout.Label(autopilot.vesselState.vesselPitch.ToString()); + //GUILayout.Label(autopilot.pitchCorrection.ToString()); GUILayout.BeginHorizontal(); GUILayout.Label("pitch PID:"); + autopilot.pitchPID.Kp = double.Parse(GUILayout.TextField(autopilot.pitchPID.Kp.ToString(), GUILayout.Width(40))); ; + autopilot.pitchPID.Ki = double.Parse(GUILayout.TextField(autopilot.pitchPID.Ki.ToString(), GUILayout.Width(40))); ; + autopilot.pitchPID.Kd = double.Parse(GUILayout.TextField(autopilot.pitchPID.Kd.ToString(), GUILayout.Width(40))); + GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); + GUILayout.Label("pitch correction PID:"); autopilot.pitchCorrectionPID.Kp = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPID.Kp.ToString(), GUILayout.Width(40))); autopilot.pitchCorrectionPID.Ki = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPID.Ki.ToString(), GUILayout.Width(40))); autopilot.pitchCorrectionPID.Kd = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPID.Kd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); + //GUILayout.Label(autopilot.desiredRoll.ToString()); + //GUILayout.Label(autopilot.vesselState.vesselRoll.ToString()); + //GUILayout.Label(autopilot.rollCorrection.ToString()); GUILayout.BeginHorizontal(); - GUILayout.Label("heading PD:"); - autopilot.headingPID.Kp = double.Parse(GUILayout.TextField(autopilot.headingPID.Kp.ToString(), GUILayout.Width(40))); - autopilot.headingPID.Kd = double.Parse(GUILayout.TextField(autopilot.headingPID.Kd.ToString(), GUILayout.Width(40))); + GUILayout.Label("roll PID:"); + autopilot.rollPID.Kp = double.Parse(GUILayout.TextField(autopilot.rollPID.Kp.ToString(), GUILayout.Width(40))); + autopilot.rollPID.Ki = double.Parse(GUILayout.TextField(autopilot.rollPID.Ki.ToString(), GUILayout.Width(40))); + autopilot.rollPID.Kd = double.Parse(GUILayout.TextField(autopilot.rollPID.Kd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); + GUILayout.Label("roll correction PID:"); + autopilot.rollCorrectionPID.Kp = double.Parse(GUILayout.TextField(autopilot.rollCorrectionPID.Kp.ToString(), GUILayout.Width(40))); + autopilot.rollCorrectionPID.Ki = double.Parse(GUILayout.TextField(autopilot.rollCorrectionPID.Ki.ToString(), GUILayout.Width(40))); + autopilot.rollCorrectionPID.Kd = double.Parse(GUILayout.TextField(autopilot.rollCorrectionPID.Kd.ToString(), GUILayout.Width(40))); + GUILayout.EndHorizontal(); + + //GUILayout.Label(autopilot.pitchPID.intAccum.ToString()); + //GUILayout.Label(autopilot.pitchCorrectionPID.intAccum.ToString()); + //GUILayout.Label(autopilot.rollPID.intAccum.ToString()); + //GUILayout.Label(autopilot.rollCorrectionPID.intAccum.ToString()); if (autopilot.enabled && autopilot.mode == MechJebModuleSpaceplaneAutopilot.Mode.HOLD && GUILayout.Button("Abort")) autopilot.Abort(); From 259eae202b8eaff12a16b0f7ea934efce89c4459 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Wed, 26 Jun 2013 01:46:43 -0700 Subject: [PATCH 20/35] Fix for rough landings Aims the craft directly down the runway before touchdown, further stabilizing the craft. Also there are some throttle changes (and likely more to come). --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index d5e4a908e..eb49edd9f 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -192,6 +192,14 @@ public void DriveAutoland(FlightCtrlState s) Vector3d vectorToWaypoint = ILSAimDirection(); double headingToWaypoint = vesselState.HeadingFromDirection(vectorToWaypoint); + //stop any rolling and aim down runway before touching down + if ((vesselState.CoM - runwayStart).magnitude < 500.0) + { + Vector3d runwayDir = runway.End(vesselState.CoM) - runway.Start(vesselState.CoM); + runwayHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(runwayDir, vesselState.east), Vector3d.Dot(runwayDir, vesselState.north)); + headingToWaypoint = runwayHeading; + } + Vector3d vectorToRunway = runwayStart - vesselState.CoM; double verticalDistanceToRunway = Vector3d.Dot(vectorToRunway, vesselState.up); double horizontalDistanceToRunway = Math.Sqrt(vectorToRunway.sqrMagnitude - verticalDistanceToRunway * verticalDistanceToRunway); @@ -282,8 +290,14 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) //if (rollCorrectionPID.intAccum < -10) rollCorrectionPID.intAccum = -10; //regulate throttle - if (pitchCorrection > throttleUpPitch) vessel.ctrlState.mainThrottle += 0.002F * (float)(pitchCorrection - throttleUpPitch) * vessel.ctrlState.mainThrottle; - if (pitchCorrection < throttleDownPitch) vessel.ctrlState.mainThrottle += 0.002F * (float)(pitchCorrection - throttleDownPitch) * vessel.ctrlState.mainThrottle; + if ((vesselState.vesselPitch - velocityPitch) > pitchPID.max * 0.95 && mode == Mode.HOLD) + vessel.ctrlState.mainThrottle = 1F; + else + { + if (pitchCorrection > throttleUpPitch) vessel.ctrlState.mainThrottle += 0.002F * (float)(pitchCorrection - throttleUpPitch) * vessel.ctrlState.mainThrottle; + if (pitchCorrection < throttleDownPitch) vessel.ctrlState.mainThrottle += 0.002F * (float)(pitchCorrection - throttleDownPitch) * vessel.ctrlState.mainThrottle; + } + } } From 90e78043e547e5613b7e0280eaa2045fbe0e1053 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Wed, 26 Jun 2013 13:25:35 -0700 Subject: [PATCH 21/35] Changes to throttle control Changed the throttle control to be more effective for craft that have smaller lift surfaces compared to their mass. --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 36 +++++++++++--------- 1 file changed, 20 insertions(+), 16 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index eb49edd9f..b88d1999f 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -13,6 +13,8 @@ public void Autoland(object controller) users.Add(controller); AutopilotOn(); maxRoll = 22.5F; + throttleUpPitch = 0.8F; + throttleDownPitch = 0.7F; mode = Mode.AUTOLAND; landed = false; loweredGear = false; @@ -24,6 +26,8 @@ public void HoldHeadingAndAltitude(object controller) users.Add(controller); AutopilotOn(); maxRoll = 22.5F; + throttleUpPitch = 0.8F; + throttleDownPitch = 0.7F; mode = Mode.HOLD; landed = false; loweredGear = false; @@ -99,7 +103,6 @@ public override void Drive(FlightCtrlState s) } } - bool gearDown = false; public float takeoffPitch = 0.5F; public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { @@ -107,27 +110,28 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { if (!autopilotOn) AutopilotOn(); - if (!gearDown) + if (!loweredGear) { - if (part.vessel.terrainAltitude < 100.0) + if ((part.vessel.altitude - part.vessel.terrainAltitude) < 100.0) { vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, true); - gearDown = true; + loweredGear = true; } } else { - if (part.vessel.terrainAltitude > 100.0) + if ((part.vessel.altitude - part.vessel.terrainAltitude) > 100.0) { vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, false); - gearDown = false; + loweredGear = false; } } //takeoff or set for flight if (landed) { - vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, false); + //vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, false); + loweredGear = true; maxRoll = 25.0F; landed = false; } @@ -140,6 +144,7 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) AutopilotOff(); landTime = vesselState.time; landed = true; + loweredGear = true; } //apply breaks a little after touchdown if (!brakes && vesselState.time > landTime + 1.0) @@ -219,6 +224,9 @@ public void DriveAutoland(FlightCtrlState s) double flightPathAngleToRunway = 180 / Math.PI * Math.Atan2(verticalDistanceToRunway, horizontalDistanceToRunway); double desiredFPA = Mathf.Clamp((float)(flightPathAngleToRunway + 3 * (flightPathAngleToRunway + glideslope)), -20.0F, 5.0F); + throttleUpPitch = 0.9F; + throttleDownPitch = 0.8F; + aimAltitude = verticalDistanceToRunway; AimVelocityVector(desiredFPA, headingToWaypoint); } @@ -259,8 +267,8 @@ public void DriveAutoland(FlightCtrlState s) public double pitchCorrection = 0; public double desiredRoll = 0; public double rollCorrection = 0; - public float throttleUpPitch = 0.5F; - public float throttleDownPitch = 0.0F; + public float throttleUpPitch = 0.6F; + public float throttleDownPitch = 0.4F; void AimVelocityVector(double desiredFpa, double desiredHeading) { if (autopilotOn) @@ -290,13 +298,9 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) //if (rollCorrectionPID.intAccum < -10) rollCorrectionPID.intAccum = -10; //regulate throttle - if ((vesselState.vesselPitch - velocityPitch) > pitchPID.max * 0.95 && mode == Mode.HOLD) - vessel.ctrlState.mainThrottle = 1F; - else - { - if (pitchCorrection > throttleUpPitch) vessel.ctrlState.mainThrottle += 0.002F * (float)(pitchCorrection - throttleUpPitch) * vessel.ctrlState.mainThrottle; - if (pitchCorrection < throttleDownPitch) vessel.ctrlState.mainThrottle += 0.002F * (float)(pitchCorrection - throttleDownPitch) * vessel.ctrlState.mainThrottle; - } + float pitchPercent = (float)((vesselState.vesselPitch - velocityPitch) / pitchPID.max); + if (pitchPercent > throttleUpPitch) vessel.ctrlState.mainThrottle += 0.001F * (float)(pitchPercent - throttleUpPitch) * vessel.ctrlState.mainThrottle; + if (pitchPercent < throttleDownPitch) vessel.ctrlState.mainThrottle += 0.001F * (float)(pitchPercent - throttleDownPitch) * vessel.ctrlState.mainThrottle; } } From 8eeac7ad9ad5f991800d4da9318c09fcc94c5bf3 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Thu, 27 Jun 2013 13:23:12 -0700 Subject: [PATCH 22/35] Low pass filter for roll correction output Added an additional low pass filter to reduce the noise coming out of the roll correction PID controller. Ideally, the input should be smoothed, but the noise was just too much for a single low pass filter to handle and much of the noise was originating from the roll control output. --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 67 +++++++++++++++++--- MechJeb2/MechJebModuleSpaceplaneGuidance.cs | 8 +-- 2 files changed, 62 insertions(+), 13 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index b88d1999f..6df1f8cdc 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -166,8 +166,8 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) vessel.ctrlState.yaw = (float)MuUtils.ClampDegrees180(targetHeading - vesselState.vesselHeading) * 0.1F; } - double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) / (CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 4)); - double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 6), (float)Math.Sin(Math.PI / 6))); + double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) / (CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 5)); + double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 7), (float)Math.Sin(Math.PI / 7))); AimVelocityVector(targetFlightPathAngle, targetHeading); } @@ -256,12 +256,33 @@ public void DriveAutoland(FlightCtrlState s) } } - public float maxRoll = 25.0F; public PIDController pitchPID = new PIDController(1, 0.3, 0.1, 25, -10); public PIDController pitchCorrectionPID = new PIDController(0.2, 0.05, 0.1, 1, -1); public PIDController rollPID = new PIDController(2.5, 0, 0.5); public PIDController rollCorrectionPID = new PIDController(0.006, 0, 0.003, 1, -1); - public LowPass180 rollLowPass = new LowPass180(0.35); + double altitudePercent = 0; + double lowPitchCorPidKd = 0.1; + double highPitchCorPidKd = 1; + double lowRollCorPidKd = 0.002; + double highRollCorPidKd = 0.03; + double lowPitchPidMax = 25; + double highPitchPidMax = 5; + double lowPitchPidMin = -10; + double highPitchPidMin = 0; + void setPIDAltitude() + { + altitudePercent = vessel.altitude / CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody); + + pitchCorrectionPID.Kd = lowPitchCorPidKd + (highPitchCorPidKd - lowPitchCorPidKd) * altitudePercent; + rollCorrectionPID.Kd = lowRollCorPidKd + (highRollCorPidKd - lowRollCorPidKd) * altitudePercent; + + pitchPID.max = lowPitchPidMax + (highPitchPidMax - lowPitchPidMax) * altitudePercent; + pitchPID.min = lowPitchPidMin + (highPitchPidMin - lowPitchPidMin) * altitudePercent; + } + + public float maxRoll = 25.0F; + public LowPass180 rollLowPass = new LowPass180(0.2); + public LowPass180 rollLowPassOutput = new LowPass180(0.1); public double noseRoll = 0; public double desiredAoA = 0; public double pitchCorrection = 0; @@ -273,12 +294,14 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) { if (autopilotOn) { + setPIDAltitude(); + //horizontal control double velocityHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.east), Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.north)); noseRoll = rollLowPass.calc(vesselState.vesselRoll); desiredRoll = -Mathf.Clamp((float)rollPID.Compute(MuUtils.ClampDegrees180(desiredHeading - velocityHeading)), -maxRoll, maxRoll); - rollCorrection = Mathf.Clamp((float)rollCorrectionPID.Compute(noseRoll - desiredRoll), (float)rollCorrectionPID.min, (float)rollCorrectionPID.max); + rollCorrection = rollLowPassOutput.calc(Mathf.Clamp((float)rollCorrectionPID.Compute(noseRoll - desiredRoll), (float)rollCorrectionPID.min, (float)rollCorrectionPID.max)); //vertical control double velocityPitch = Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI); @@ -299,15 +322,28 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) //regulate throttle float pitchPercent = (float)((vesselState.vesselPitch - velocityPitch) / pitchPID.max); - if (pitchPercent > throttleUpPitch) vessel.ctrlState.mainThrottle += 0.001F * (float)(pitchPercent - throttleUpPitch) * vessel.ctrlState.mainThrottle; - if (pitchPercent < throttleDownPitch) vessel.ctrlState.mainThrottle += 0.001F * (float)(pitchPercent - throttleDownPitch) * vessel.ctrlState.mainThrottle; + if (pitchPercent > throttleUpPitch) vessel.ctrlState.mainThrottle += 0.01F * (float)(pitchPercent - throttleUpPitch) * vessel.ctrlState.mainThrottle; + if (pitchPercent < throttleDownPitch && vessel.altitude < 0.4 * CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody)) vessel.ctrlState.mainThrottle += 0.001F * (float)(pitchPercent - throttleDownPitch) * vessel.ctrlState.mainThrottle; } } public class LowPass360 { - public double y_old, alpha, timeConstant, dt; + private double y_old, alpha, timeConstant, dt; + + public double TimeConstant + { + get { return this.timeConstant; } + set + { + if (value != timeConstant) + { + this.timeConstant = value; + initialize(); + } + } + } public LowPass360(double timeConst) { @@ -345,7 +381,20 @@ public void clear() public class LowPass180 { - public double y_old, alpha, timeConstant, dt; + private double y_old, alpha, timeConstant, dt; + + public double TimeConstant + { + get { return this.timeConstant; } + set + { + if (value != timeConstant) + { + this.timeConstant = value; + initialize(); + } + } + } public LowPass180(double timeConst) { diff --git a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs index 27d08c13e..4563dc0d5 100644 --- a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs +++ b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs @@ -65,10 +65,12 @@ protected override void WindowGUI(int windowID) //GUILayout.Label(autopilot.desiredAoA.ToString()); //GUILayout.Label(autopilot.vesselState.vesselPitch.ToString()); //GUILayout.Label(autopilot.pitchCorrection.ToString()); + //autopilot.rollLowPass.TimeConstant = double.Parse(GUILayout.TextField(autopilot.rollLowPass.TimeConstant.ToString(), GUILayout.Width(40))); + //autopilot.rollLowPassOutput.TimeConstant = double.Parse(GUILayout.TextField(autopilot.rollLowPassOutput.TimeConstant.ToString(), GUILayout.Width(40))); GUILayout.BeginHorizontal(); GUILayout.Label("pitch PID:"); - autopilot.pitchPID.Kp = double.Parse(GUILayout.TextField(autopilot.pitchPID.Kp.ToString(), GUILayout.Width(40))); ; - autopilot.pitchPID.Ki = double.Parse(GUILayout.TextField(autopilot.pitchPID.Ki.ToString(), GUILayout.Width(40))); ; + autopilot.pitchPID.Kp = double.Parse(GUILayout.TextField(autopilot.pitchPID.Kp.ToString(), GUILayout.Width(40))); + autopilot.pitchPID.Ki = double.Parse(GUILayout.TextField(autopilot.pitchPID.Ki.ToString(), GUILayout.Width(40))); autopilot.pitchPID.Kd = double.Parse(GUILayout.TextField(autopilot.pitchPID.Kd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); @@ -124,8 +126,6 @@ public override void OnFixedUpdate() } } - - public override void OnStart(PartModule.StartState state) { autopilot = core.GetComputerModule(); From ca0bcfc0c3bc669ec5f8350d8d04b4ca4d2623e7 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Fri, 28 Jun 2013 12:46:16 -0700 Subject: [PATCH 23/35] Altered PID settings PID settings now change based on a planes mass. --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 40 +++++++++++++------- MechJeb2/MechJebModuleSpaceplaneGuidance.cs | 8 ++-- 2 files changed, 32 insertions(+), 16 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 6df1f8cdc..a03d22fb8 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -212,9 +212,9 @@ public void DriveAutoland(FlightCtrlState s) if (horizontalDistanceToRunway > 10000) { aimAltitude = 1500; - double horizontalDifference = 10000; - if (horizontalDistanceToRunway > 30000) { aimAltitude = 3000; horizontalDifference = 30000; } - if (horizontalDistanceToRunway > 50000) { aimAltitude = 8000; horizontalDifference = 50000; } + double horizontalDifference = 9900; + if (horizontalDistanceToRunway > 30000) { aimAltitude = 3000; horizontalDifference = 29900; } + if (horizontalDistanceToRunway > 50000) { aimAltitude = 8000; horizontalDifference = 49900; } double setupFPA = 180 / Math.PI * Math.Atan2(aimAltitude - vessel.altitude, horizontalDistanceToRunway - horizontalDifference); AimVelocityVector(setupFPA, headingToWaypoint); @@ -256,33 +256,47 @@ public void DriveAutoland(FlightCtrlState s) } } - public PIDController pitchPID = new PIDController(1, 0.3, 0.1, 25, -10); + public PIDController pitchPID = new PIDController(0.6, 0.2, 0.2, 25, -10); public PIDController pitchCorrectionPID = new PIDController(0.2, 0.05, 0.1, 1, -1); public PIDController rollPID = new PIDController(2.5, 0, 0.5); public PIDController rollCorrectionPID = new PIDController(0.006, 0, 0.003, 1, -1); double altitudePercent = 0; - double lowPitchCorPidKd = 0.1; + double pitchCorrectionPidKp = 0.2; + //double pitchCorrectionPidKi = 0.05; + //double pitchCorrectionPidKd = 0.2; + double lowPitchCorPidKd = 0.07; double highPitchCorPidKd = 1; - double lowRollCorPidKd = 0.002; - double highRollCorPidKd = 0.03; + double rollPidKp = 2.5; + double rollPidKd = 0.5; + double lowRollCorPidKd = 0.001; + double highRollCorPidKd = 0.01; double lowPitchPidMax = 25; double highPitchPidMax = 5; double lowPitchPidMin = -10; double highPitchPidMin = 0; + double controlGain = 1; void setPIDAltitude() { altitudePercent = vessel.altitude / CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody); + controlGain = 0.1 + vesselState.mass / 53; + pitchCorrectionPID.Kd = lowPitchCorPidKd + (highPitchCorPidKd - lowPitchCorPidKd) * altitudePercent; rollCorrectionPID.Kd = lowRollCorPidKd + (highRollCorPidKd - lowRollCorPidKd) * altitudePercent; + pitchCorrectionPID.Kp = pitchCorrectionPidKp * controlGain; + pitchCorrectionPID.Kd *= controlGain; + + rollPID.Kp = rollPidKp * controlGain; + rollPID.Kd = rollPidKd * controlGain; + pitchPID.max = lowPitchPidMax + (highPitchPidMax - lowPitchPidMax) * altitudePercent; pitchPID.min = lowPitchPidMin + (highPitchPidMin - lowPitchPidMin) * altitudePercent; } public float maxRoll = 25.0F; - public LowPass180 rollLowPass = new LowPass180(0.2); - public LowPass180 rollLowPassOutput = new LowPass180(0.1); + public LowPass180 rollLowPass = new LowPass180(0.1); + public LowPass180 pitchLowPass = new LowPass180(0.1); public double noseRoll = 0; public double desiredAoA = 0; public double pitchCorrection = 0; @@ -301,10 +315,10 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.north)); noseRoll = rollLowPass.calc(vesselState.vesselRoll); desiredRoll = -Mathf.Clamp((float)rollPID.Compute(MuUtils.ClampDegrees180(desiredHeading - velocityHeading)), -maxRoll, maxRoll); - rollCorrection = rollLowPassOutput.calc(Mathf.Clamp((float)rollCorrectionPID.Compute(noseRoll - desiredRoll), (float)rollCorrectionPID.min, (float)rollCorrectionPID.max)); + rollCorrection = Mathf.Clamp((float)rollCorrectionPID.Compute(noseRoll - desiredRoll), (float)rollCorrectionPID.min, (float)rollCorrectionPID.max); //vertical control - double velocityPitch = Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI); + double velocityPitch = pitchLowPass.calc(Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI)); desiredAoA = Mathf.Clamp((float)pitchPID.Compute(desiredFpa - velocityPitch), (float)pitchPID.min, (float)pitchPID.max); pitchCorrection = Mathf.Clamp((float)pitchCorrectionPID.Compute(desiredAoA - (vesselState.vesselPitch - velocityPitch)), (float)pitchCorrectionPID.min, (float)pitchCorrectionPID.max); @@ -363,7 +377,7 @@ public double calc(double x) if (dt != TimeWarp.fixedDeltaTime) { initialize(); - y_old = x; + y_old = 0; } else { @@ -414,7 +428,7 @@ public double calc(double x) if (dt != TimeWarp.fixedDeltaTime) { initialize(); - y_old = x; + y_old = 0; } else { diff --git a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs index 4563dc0d5..18b5cbb00 100644 --- a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs +++ b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs @@ -62,11 +62,13 @@ protected override void WindowGUI(int windowID) GUILayout.EndHorizontal(); //temp + //GUILayout.Label(autopilot.noseRoll.ToString()); + //autopilot.rollLowPass.TimeConstant = double.Parse(GUILayout.TextField(autopilot.rollLowPass.TimeConstant.ToString(), GUILayout.Width(40))); + //autopilot.pitchLowPass.TimeConstant = double.Parse(GUILayout.TextField(autopilot.pitchLowPass.TimeConstant.ToString(), GUILayout.Width(40))); + /* //GUILayout.Label(autopilot.desiredAoA.ToString()); //GUILayout.Label(autopilot.vesselState.vesselPitch.ToString()); //GUILayout.Label(autopilot.pitchCorrection.ToString()); - //autopilot.rollLowPass.TimeConstant = double.Parse(GUILayout.TextField(autopilot.rollLowPass.TimeConstant.ToString(), GUILayout.Width(40))); - //autopilot.rollLowPassOutput.TimeConstant = double.Parse(GUILayout.TextField(autopilot.rollLowPassOutput.TimeConstant.ToString(), GUILayout.Width(40))); GUILayout.BeginHorizontal(); GUILayout.Label("pitch PID:"); autopilot.pitchPID.Kp = double.Parse(GUILayout.TextField(autopilot.pitchPID.Kp.ToString(), GUILayout.Width(40))); @@ -95,7 +97,7 @@ protected override void WindowGUI(int windowID) autopilot.rollCorrectionPID.Ki = double.Parse(GUILayout.TextField(autopilot.rollCorrectionPID.Ki.ToString(), GUILayout.Width(40))); autopilot.rollCorrectionPID.Kd = double.Parse(GUILayout.TextField(autopilot.rollCorrectionPID.Kd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); - + */ //GUILayout.Label(autopilot.pitchPID.intAccum.ToString()); //GUILayout.Label(autopilot.pitchCorrectionPID.intAccum.ToString()); //GUILayout.Label(autopilot.rollPID.intAccum.ToString()); From 656f75c2a9b3044b54d42c1bef6eda5492a278c2 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Sun, 30 Jun 2013 00:41:41 -0700 Subject: [PATCH 24/35] Yaw control and more noise suppression Added a yaw PID controller to further help smooth out the flight. Added some features to the low pass filters and implemented more of them through the roll/yaw control loop. --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 162 +++++++++++++++---- MechJeb2/MechJebModuleSpaceplaneGuidance.cs | 22 ++- 2 files changed, 147 insertions(+), 37 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index a03d22fb8..9449f0ccf 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -12,7 +12,7 @@ public void Autoland(object controller) { users.Add(controller); AutopilotOn(); - maxRoll = 22.5F; + maxRoll = 30F; throttleUpPitch = 0.8F; throttleDownPitch = 0.7F; mode = Mode.AUTOLAND; @@ -25,7 +25,7 @@ public void HoldHeadingAndAltitude(object controller) { users.Add(controller); AutopilotOn(); - maxRoll = 22.5F; + maxRoll = 30F; throttleUpPitch = 0.8F; throttleDownPitch = 0.7F; mode = Mode.HOLD; @@ -132,7 +132,6 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { //vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, false); loweredGear = true; - maxRoll = 25.0F; landed = false; } } @@ -176,6 +175,8 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) public double aimAltitude = 0; public double distanceFrom = 0; public double runwayHeading = 0; + public enum HeadingState { RIGHT, LEFT, OFF }; + public HeadingState autolandHeadingState = HeadingState.OFF; public void DriveAutoland(FlightCtrlState s) { if (!part.vessel.Landed) @@ -196,6 +197,27 @@ public void DriveAutoland(FlightCtrlState s) Vector3d vectorToWaypoint = ILSAimDirection(); double headingToWaypoint = vesselState.HeadingFromDirection(vectorToWaypoint); + if (Math.Abs(MuUtils.ClampDegrees180(headingToWaypoint - vesselState.vesselHeading)) > 170) + { + //make sure the heading doesn't keep switching from side to side + switch (autolandHeadingState) + { + case HeadingState.RIGHT: + headingToWaypoint = MuUtils.ClampDegrees360(vesselState.vesselHeading + 90); + break; + case HeadingState.LEFT: + headingToWaypoint = MuUtils.ClampDegrees360(vesselState.vesselHeading - 90); + break; + case HeadingState.OFF: + if (headingToWaypoint - vesselState.vesselHeading > 0) + autolandHeadingState = HeadingState.RIGHT; + else + autolandHeadingState = HeadingState.LEFT; + break; + } + } + else + autolandHeadingState = HeadingState.OFF; //stop any rolling and aim down runway before touching down if ((vesselState.CoM - runwayStart).magnitude < 500.0) @@ -256,19 +278,20 @@ public void DriveAutoland(FlightCtrlState s) } } - public PIDController pitchPID = new PIDController(0.6, 0.2, 0.2, 25, -10); + public PIDController pitchPID = new PIDController(0.6, 0.2, 0.6, 25, -10); public PIDController pitchCorrectionPID = new PIDController(0.2, 0.05, 0.1, 1, -1); public PIDController rollPID = new PIDController(2.5, 0, 0.5); - public PIDController rollCorrectionPID = new PIDController(0.006, 0, 0.003, 1, -1); + public PIDController rollCorrectionPID = new PIDController(0.002, 0, 0.002, 1, -1); + public PIDController yawCorrectionPID = new PIDController(0.1, 0, 0.1, 1, -1); double altitudePercent = 0; double pitchCorrectionPidKp = 0.2; //double pitchCorrectionPidKi = 0.05; //double pitchCorrectionPidKd = 0.2; double lowPitchCorPidKd = 0.07; double highPitchCorPidKd = 1; - double rollPidKp = 2.5; - double rollPidKd = 0.5; - double lowRollCorPidKd = 0.001; + public double rollPidKp = 10; + public double rollPidKd = 5; + double lowRollCorPidKd = 0.002; double highRollCorPidKd = 0.01; double lowPitchPidMax = 25; double highPitchPidMax = 5; @@ -294,36 +317,52 @@ void setPIDAltitude() pitchPID.min = lowPitchPidMin + (highPitchPidMin - lowPitchPidMin) * altitudePercent; } - public float maxRoll = 25.0F; - public LowPass180 rollLowPass = new LowPass180(0.1); - public LowPass180 pitchLowPass = new LowPass180(0.1); + public float maxRoll = 30.0F; + public LowPass180 rollLowPass = new LowPass180(0.1, 5, 0); + public LowPass180 desiredRollLowPass = new LowPass180(0.1, 5, 10); + public LowPass180 rollCorrectionLowPass = new LowPass180(0.1, 5, 20); + public LowPass180 yawCorrectionLowPass = new LowPass180(0.1, 5, 20); + public LowPass180 pitchLowPass = new LowPass180(0.1, 5, 0); + public LowPass360 headingLowPass = new LowPass360(0.1, 5, 0); + public LowPass360 velocityHeadingLowPass = new LowPass360(0.1, 5, 0); public double noseRoll = 0; + public double noseYaw = 0; public double desiredAoA = 0; public double pitchCorrection = 0; public double desiredRoll = 0; + public double desiredYaw = 0; public double rollCorrection = 0; + public double yawCorrection = 0; public float throttleUpPitch = 0.6F; public float throttleDownPitch = 0.4F; + public double velocityHeading = 0; + public double velocityHeadingTest = 0; + public double velocityPitch = 0; void AimVelocityVector(double desiredFpa, double desiredHeading) { if (autopilotOn) { setPIDAltitude(); + velocityHeading = velocityHeadingLowPass.calc(180 / Math.PI * Math.Atan2(Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.east), + Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.north))); + velocityPitch = pitchLowPass.calc(Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI)); + //horizontal control - double velocityHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.east), - Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.north)); noseRoll = rollLowPass.calc(vesselState.vesselRoll); - desiredRoll = -Mathf.Clamp((float)rollPID.Compute(MuUtils.ClampDegrees180(desiredHeading - velocityHeading)), -maxRoll, maxRoll); - rollCorrection = Mathf.Clamp((float)rollCorrectionPID.Compute(noseRoll - desiredRoll), (float)rollCorrectionPID.min, (float)rollCorrectionPID.max); + noseYaw = MuUtils.ClampDegrees180(headingLowPass.calc(vesselState.vesselHeading) - velocityHeading); + desiredRoll = desiredRollLowPass.calc(- Mathf.Clamp((float)rollPID.Compute(MuUtils.ClampDegrees180(desiredHeading - velocityHeading)), -maxRoll, maxRoll)); + desiredYaw = -desiredRoll / 4;// -(vesselState.vesselPitch - velocityPitch) * (desiredRoll / 60); + rollCorrection = rollCorrectionLowPass.calc(Mathf.Clamp((float)rollCorrectionPID.Compute(noseRoll - desiredRoll), (float)rollCorrectionPID.min, (float)rollCorrectionPID.max)); + yawCorrection = yawCorrectionLowPass.calc(Mathf.Clamp((float)yawCorrectionPID.Compute(desiredYaw - noseYaw), (float)yawCorrectionPID.min, (float)yawCorrectionPID.max)); //vertical control - double velocityPitch = pitchLowPass.calc(Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI)); desiredAoA = Mathf.Clamp((float)pitchPID.Compute(desiredFpa - velocityPitch), (float)pitchPID.min, (float)pitchPID.max); pitchCorrection = Mathf.Clamp((float)pitchCorrectionPID.Compute(desiredAoA - (vesselState.vesselPitch - velocityPitch)), (float)pitchCorrectionPID.min, (float)pitchCorrectionPID.max); vessel.ctrlState.roll = (float)rollCorrection; vessel.ctrlState.pitch = (float)pitchCorrection; + vessel.ctrlState.yaw = (float)yawCorrection; if (pitchPID.intAccum > 100) pitchPID.intAccum = 100; if (pitchPID.intAccum < -100) pitchPID.intAccum = -100; @@ -344,7 +383,8 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) public class LowPass360 { - private double y_old, alpha, timeConstant, dt; + private double y_old, alpha, timeConstant, dt, passGap; + int calcCount, delayCount; public double TimeConstant { @@ -364,26 +404,52 @@ public LowPass360(double timeConst) this.timeConstant = timeConst; clear(); initialize(); + this.passGap = 0; + this.delayCount = 0; + } + + public LowPass360(double timeConst, double passGap, int delayCount) + { + this.timeConstant = timeConst; + clear(); + initialize(); + this.passGap = passGap; + this.delayCount = delayCount; } private void initialize() { dt = TimeWarp.fixedDeltaTime; this.alpha = TimeWarp.fixedDeltaTime / (timeConstant - TimeWarp.fixedDeltaTime); + calcCount = 0; + y_old = 0; } public double calc(double x) { - if (dt != TimeWarp.fixedDeltaTime) - { - initialize(); - y_old = 0; - } - else + if (calcCount >= delayCount) { - y_old = MuUtils.ClampDegrees360(y_old + alpha * MuUtils.ClampDegrees180(x - y_old)); + x = MuUtils.ClampDegrees360(x); + if (dt != TimeWarp.fixedDeltaTime) + { + initialize(); + y_old = x; + } + else + { + if (calcCount > 10) + { + if (MuUtils.ClampDegrees180(x - y_old) > passGap) + x = MuUtils.ClampDegrees360(y_old + passGap); + else if (MuUtils.ClampDegrees180(x - y_old) < -passGap) + x = MuUtils.ClampDegrees360(y_old - passGap); + } + y_old = MuUtils.ClampDegrees360(y_old + alpha * MuUtils.ClampDegrees180(x - y_old)); + } } + if (calcCount <= 100 || calcCount <= delayCount) calcCount++; + return y_old; } @@ -395,7 +461,8 @@ public void clear() public class LowPass180 { - private double y_old, alpha, timeConstant, dt; + private double y_old, alpha, timeConstant, dt, passGap; + int calcCount, delayCount; public double TimeConstant { @@ -415,26 +482,57 @@ public LowPass180(double timeConst) this.timeConstant = timeConst; clear(); initialize(); + this.passGap = 0; + this.delayCount = 0; + } + + public LowPass180(double timeConst, double passGap, int delayCount) + { + this.timeConstant = timeConst; + clear(); + initialize(); + this.passGap = passGap; + this.delayCount = delayCount; } + private void initialize() { dt = TimeWarp.fixedDeltaTime; this.alpha = TimeWarp.fixedDeltaTime / (timeConstant - TimeWarp.fixedDeltaTime); + calcCount = 0; + y_old = 0; } public double calc(double x) { - if (dt != TimeWarp.fixedDeltaTime) + if (calcCount >= delayCount) { - initialize(); - y_old = 0; - } - else - { - y_old = MuUtils.ClampDegrees180(y_old + alpha * MuUtils.ClampDegrees180(x - y_old)); + MuUtils.ClampDegrees180(x); + if (dt != TimeWarp.fixedDeltaTime) + { + initialize(); + y_old = x; + } + else + { + if (calcCount > 100) + { + if (calcCount > 10) + { + if (MuUtils.ClampDegrees180(x - y_old) > passGap) + x = MuUtils.ClampDegrees180(y_old + passGap); + else if (MuUtils.ClampDegrees180(x - y_old) < -passGap) + x = MuUtils.ClampDegrees180(y_old - passGap); + } + y_old = MuUtils.ClampDegrees360(y_old + alpha * MuUtils.ClampDegrees180(x - y_old)); + } + y_old = MuUtils.ClampDegrees180(y_old + alpha * MuUtils.ClampDegrees180(x - y_old)); + } } + if (calcCount <= 100 || calcCount <= delayCount) calcCount++; + return y_old; } diff --git a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs index 18b5cbb00..6283ba9eb 100644 --- a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs +++ b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs @@ -65,7 +65,7 @@ protected override void WindowGUI(int windowID) //GUILayout.Label(autopilot.noseRoll.ToString()); //autopilot.rollLowPass.TimeConstant = double.Parse(GUILayout.TextField(autopilot.rollLowPass.TimeConstant.ToString(), GUILayout.Width(40))); //autopilot.pitchLowPass.TimeConstant = double.Parse(GUILayout.TextField(autopilot.pitchLowPass.TimeConstant.ToString(), GUILayout.Width(40))); - /* + //GUILayout.Label(autopilot.desiredAoA.ToString()); //GUILayout.Label(autopilot.vesselState.vesselPitch.ToString()); //GUILayout.Label(autopilot.pitchCorrection.ToString()); @@ -82,14 +82,17 @@ protected override void WindowGUI(int windowID) autopilot.pitchCorrectionPID.Kd = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPID.Kd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); - //GUILayout.Label(autopilot.desiredRoll.ToString()); + GUILayout.Label(autopilot.desiredRoll.ToString()); + GUILayout.Label(autopilot.autolandHeadingState.ToString()); + GUILayout.Label(autopilot.velocityHeading.ToString()); + GUILayout.Label(autopilot.velocityHeadingTest.ToString()); //GUILayout.Label(autopilot.vesselState.vesselRoll.ToString()); //GUILayout.Label(autopilot.rollCorrection.ToString()); GUILayout.BeginHorizontal(); GUILayout.Label("roll PID:"); - autopilot.rollPID.Kp = double.Parse(GUILayout.TextField(autopilot.rollPID.Kp.ToString(), GUILayout.Width(40))); + autopilot.rollPidKp = double.Parse(GUILayout.TextField(autopilot.rollPidKp.ToString(), GUILayout.Width(40))); autopilot.rollPID.Ki = double.Parse(GUILayout.TextField(autopilot.rollPID.Ki.ToString(), GUILayout.Width(40))); - autopilot.rollPID.Kd = double.Parse(GUILayout.TextField(autopilot.rollPID.Kd.ToString(), GUILayout.Width(40))); + autopilot.rollPidKd = double.Parse(GUILayout.TextField(autopilot.rollPidKd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("roll correction PID:"); @@ -97,7 +100,16 @@ protected override void WindowGUI(int windowID) autopilot.rollCorrectionPID.Ki = double.Parse(GUILayout.TextField(autopilot.rollCorrectionPID.Ki.ToString(), GUILayout.Width(40))); autopilot.rollCorrectionPID.Kd = double.Parse(GUILayout.TextField(autopilot.rollCorrectionPID.Kd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); - */ + + GUILayout.Label(autopilot.desiredYaw.ToString()); + GUILayout.Label(autopilot.noseYaw.ToString()); + GUILayout.BeginHorizontal(); + GUILayout.Label("yaw correction PID:"); + autopilot.yawCorrectionPID.Kp = double.Parse(GUILayout.TextField(autopilot.yawCorrectionPID.Kp.ToString(), GUILayout.Width(40))); + autopilot.yawCorrectionPID.Ki = double.Parse(GUILayout.TextField(autopilot.yawCorrectionPID.Ki.ToString(), GUILayout.Width(40))); + autopilot.yawCorrectionPID.Kd = double.Parse(GUILayout.TextField(autopilot.yawCorrectionPID.Kd.ToString(), GUILayout.Width(40))); + GUILayout.EndHorizontal(); + //GUILayout.Label(autopilot.pitchPID.intAccum.ToString()); //GUILayout.Label(autopilot.pitchCorrectionPID.intAccum.ToString()); //GUILayout.Label(autopilot.rollPID.intAccum.ToString()); From cd7da56cea8de7b49e00af58b6810fe828280089 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Mon, 1 Jul 2013 01:28:08 -0700 Subject: [PATCH 25/35] Temporary fix for rolling just after takeoff --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 165 +++++++++++-------- MechJeb2/MechJebModuleSpaceplaneGuidance.cs | 23 ++- 2 files changed, 107 insertions(+), 81 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 9449f0ccf..5b6ea766b 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -38,6 +38,14 @@ public void HoldHeadingAndAltitude(object controller) public void AutopilotOn() { autopilotOn = true; + rollLowPass.initialize(); + desiredRollLowPass.initialize(); + rollCorrectionLowPass.initialize(); + yawCorrectionLowPass.initialize(); + velocityPitchLowPass.initialize(); + desiredAoALowPass.initialize(); + headingLowPass.initialize(); + velocityHeadingLowPass.initialize(); } public void AutopilotOff() @@ -167,6 +175,10 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) / (CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 5)); double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 7), (float)Math.Sin(Math.PI / 7))); + + double heading = targetHeading; + //if (loweredGear == true) heading = vesselState.vesselHeading; + AimVelocityVector(targetFlightPathAngle, targetHeading); } @@ -279,52 +291,60 @@ public void DriveAutoland(FlightCtrlState s) } public PIDController pitchPID = new PIDController(0.6, 0.2, 0.6, 25, -10); - public PIDController pitchCorrectionPID = new PIDController(0.2, 0.05, 0.1, 1, -1); - public PIDController rollPID = new PIDController(2.5, 0, 0.5); - public PIDController rollCorrectionPID = new PIDController(0.002, 0, 0.002, 1, -1); + public PIDController pitchCorrectionPID = new PIDController(0.1, 0.05, 0.1, 1, -1); + public PIDController rollPID = new PIDController(10, 0, 5); + public PIDController rollCorrectionPID = new PIDController(0.01, 0, 0.01, 1, -1); public PIDController yawCorrectionPID = new PIDController(0.1, 0, 0.1, 1, -1); double altitudePercent = 0; - double pitchCorrectionPidKp = 0.2; - //double pitchCorrectionPidKi = 0.05; - //double pitchCorrectionPidKd = 0.2; - double lowPitchCorPidKd = 0.07; - double highPitchCorPidKd = 1; - public double rollPidKp = 10; - public double rollPidKd = 5; - double lowRollCorPidKd = 0.002; - double highRollCorPidKd = 0.01; - double lowPitchPidMax = 25; - double highPitchPidMax = 5; - double lowPitchPidMin = -10; - double highPitchPidMin = 0; + public double pitchCorrectionPidKp = 0.2; + public double lowPitchCorPidKd = 0.2; + public double highPitchCorPidKd = 1; + + public double rollCorrectionPidKp = 0.005; + public double lowRollCorPidKd = 0.005; + public double highRollCorPidKd = 0.025; + + public double yawCorrectionPidKp = 0.05; + public double lowYawCorPidKd = 0.05; + public double highYawCorPidKd = 0.1; + + public double lowPitchPidMax = 25; + public double highPitchPidMax = 5; + public double lowPitchPidMin = -10; + public double highPitchPidMin = 0; double controlGain = 1; void setPIDAltitude() { altitudePercent = vessel.altitude / CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody); - controlGain = 0.1 + vesselState.mass / 53; + controlGain = vesselState.mass / 50; pitchCorrectionPID.Kd = lowPitchCorPidKd + (highPitchCorPidKd - lowPitchCorPidKd) * altitudePercent; rollCorrectionPID.Kd = lowRollCorPidKd + (highRollCorPidKd - lowRollCorPidKd) * altitudePercent; + yawCorrectionPID.Kd = lowYawCorPidKd + (highYawCorPidKd - lowYawCorPidKd) * altitudePercent; pitchCorrectionPID.Kp = pitchCorrectionPidKp * controlGain; pitchCorrectionPID.Kd *= controlGain; - rollPID.Kp = rollPidKp * controlGain; - rollPID.Kd = rollPidKd * controlGain; + rollCorrectionPID.Kp = rollCorrectionPidKp * controlGain; + rollCorrectionPID.Kd *= controlGain; + + yawCorrectionPID.Kp = yawCorrectionPidKp * controlGain; + yawCorrectionPID.Kd *= controlGain; pitchPID.max = lowPitchPidMax + (highPitchPidMax - lowPitchPidMax) * altitudePercent; pitchPID.min = lowPitchPidMin + (highPitchPidMin - lowPitchPidMin) * altitudePercent; } public float maxRoll = 30.0F; - public LowPass180 rollLowPass = new LowPass180(0.1, 5, 0); - public LowPass180 desiredRollLowPass = new LowPass180(0.1, 5, 10); - public LowPass180 rollCorrectionLowPass = new LowPass180(0.1, 5, 20); - public LowPass180 yawCorrectionLowPass = new LowPass180(0.1, 5, 20); - public LowPass180 pitchLowPass = new LowPass180(0.1, 5, 0); - public LowPass360 headingLowPass = new LowPass360(0.1, 5, 0); - public LowPass360 velocityHeadingLowPass = new LowPass360(0.1, 5, 0); + public LowPass180 rollLowPass = new LowPass180(0.15, 5, 0); + public LowPass180 desiredRollLowPass = new LowPass180(0.1, 5, 30); + public LowPass180 rollCorrectionLowPass = new LowPass180(0.1, 5, 60); + public LowPass180 yawCorrectionLowPass = new LowPass180(0.1, 5, 60); + public LowPass180 velocityPitchLowPass = new LowPass180(0.15, 5, 0); + public LowPass180 desiredAoALowPass = new LowPass180(0.1, 5, 0); + public LowPass360 headingLowPass = new LowPass360(0.15, 5, 0); + public LowPass360 velocityHeadingLowPass = new LowPass360(0.15, 5, 0); public double noseRoll = 0; public double noseYaw = 0; public double desiredAoA = 0; @@ -346,18 +366,18 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) velocityHeading = velocityHeadingLowPass.calc(180 / Math.PI * Math.Atan2(Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.east), Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.north))); - velocityPitch = pitchLowPass.calc(Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI)); + velocityPitch = velocityPitchLowPass.calc(Math.Atan2(vesselState.speedVertical, vesselState.speedSurface) * (180.0 / Math.PI)); //horizontal control noseRoll = rollLowPass.calc(vesselState.vesselRoll); noseYaw = MuUtils.ClampDegrees180(headingLowPass.calc(vesselState.vesselHeading) - velocityHeading); desiredRoll = desiredRollLowPass.calc(- Mathf.Clamp((float)rollPID.Compute(MuUtils.ClampDegrees180(desiredHeading - velocityHeading)), -maxRoll, maxRoll)); - desiredYaw = -desiredRoll / 4;// -(vesselState.vesselPitch - velocityPitch) * (desiredRoll / 60); + desiredYaw = -desiredRoll / 3;// -(vesselState.vesselPitch - velocityPitch) * (desiredRoll / 60); rollCorrection = rollCorrectionLowPass.calc(Mathf.Clamp((float)rollCorrectionPID.Compute(noseRoll - desiredRoll), (float)rollCorrectionPID.min, (float)rollCorrectionPID.max)); yawCorrection = yawCorrectionLowPass.calc(Mathf.Clamp((float)yawCorrectionPID.Compute(desiredYaw - noseYaw), (float)yawCorrectionPID.min, (float)yawCorrectionPID.max)); //vertical control - desiredAoA = Mathf.Clamp((float)pitchPID.Compute(desiredFpa - velocityPitch), (float)pitchPID.min, (float)pitchPID.max); + desiredAoA = desiredAoALowPass.calc(Mathf.Clamp((float)pitchPID.Compute(desiredFpa - velocityPitch), (float)pitchPID.min, (float)pitchPID.max)); pitchCorrection = Mathf.Clamp((float)pitchCorrectionPID.Compute(desiredAoA - (vesselState.vesselPitch - velocityPitch)), (float)pitchCorrectionPID.min, (float)pitchCorrectionPID.max); vessel.ctrlState.roll = (float)rollCorrection; @@ -386,6 +406,8 @@ public class LowPass360 private double y_old, alpha, timeConstant, dt, passGap; int calcCount, delayCount; + public double lastResult { get { return y_old; } } + public double TimeConstant { get { return this.timeConstant; } @@ -417,7 +439,7 @@ public LowPass360(double timeConst, double passGap, int delayCount) this.delayCount = delayCount; } - private void initialize() + public void initialize() { dt = TimeWarp.fixedDeltaTime; this.alpha = TimeWarp.fixedDeltaTime / (timeConstant - TimeWarp.fixedDeltaTime); @@ -427,28 +449,27 @@ private void initialize() public double calc(double x) { - if (calcCount >= delayCount) + x = MuUtils.ClampDegrees360(x); + if (dt != TimeWarp.fixedDeltaTime) { - x = MuUtils.ClampDegrees360(x); - if (dt != TimeWarp.fixedDeltaTime) - { - initialize(); - y_old = x; - } - else - { - if (calcCount > 10) - { - if (MuUtils.ClampDegrees180(x - y_old) > passGap) - x = MuUtils.ClampDegrees360(y_old + passGap); - else if (MuUtils.ClampDegrees180(x - y_old) < -passGap) - x = MuUtils.ClampDegrees360(y_old - passGap); - } - y_old = MuUtils.ClampDegrees360(y_old + alpha * MuUtils.ClampDegrees180(x - y_old)); - } + initialize(); + y_old = x; } + else + { + if (MuUtils.ClampDegrees180(x - y_old) > passGap) + x = MuUtils.ClampDegrees360(y_old + passGap); + else if (MuUtils.ClampDegrees180(x - y_old) < -passGap) + x = MuUtils.ClampDegrees360(y_old - passGap); - if (calcCount <= 100 || calcCount <= delayCount) calcCount++; + y_old = MuUtils.ClampDegrees360(y_old + alpha * MuUtils.ClampDegrees180(x - y_old)); + } + + if (calcCount <= delayCount) + { + calcCount++; + return 0; + } return y_old; } @@ -464,6 +485,8 @@ public class LowPass180 private double y_old, alpha, timeConstant, dt, passGap; int calcCount, delayCount; + public double lastResult { get { return y_old; } } + public double TimeConstant { get { return this.timeConstant; } @@ -496,7 +519,7 @@ public LowPass180(double timeConst, double passGap, int delayCount) } - private void initialize() + public void initialize() { dt = TimeWarp.fixedDeltaTime; this.alpha = TimeWarp.fixedDeltaTime / (timeConstant - TimeWarp.fixedDeltaTime); @@ -506,32 +529,28 @@ private void initialize() public double calc(double x) { - if (calcCount >= delayCount) + MuUtils.ClampDegrees180(x); + if (dt != TimeWarp.fixedDeltaTime) { - MuUtils.ClampDegrees180(x); - if (dt != TimeWarp.fixedDeltaTime) - { - initialize(); - y_old = x; - } - else - { - if (calcCount > 100) - { - if (calcCount > 10) - { - if (MuUtils.ClampDegrees180(x - y_old) > passGap) - x = MuUtils.ClampDegrees180(y_old + passGap); - else if (MuUtils.ClampDegrees180(x - y_old) < -passGap) - x = MuUtils.ClampDegrees180(y_old - passGap); - } - y_old = MuUtils.ClampDegrees360(y_old + alpha * MuUtils.ClampDegrees180(x - y_old)); - } - y_old = MuUtils.ClampDegrees180(y_old + alpha * MuUtils.ClampDegrees180(x - y_old)); - } + initialize(); + y_old = x; + } + else + { + if (MuUtils.ClampDegrees180(x - y_old) > passGap) + x = MuUtils.ClampDegrees180(y_old + passGap); + else if (MuUtils.ClampDegrees180(x - y_old) < -passGap) + x = MuUtils.ClampDegrees180(y_old - passGap); + + y_old = MuUtils.ClampDegrees180(y_old + alpha * MuUtils.ClampDegrees180(x - y_old)); } - if (calcCount <= 100 || calcCount <= delayCount) calcCount++; + + if (calcCount <= delayCount) + { + calcCount++; + return 0; + } return y_old; } diff --git a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs index 6283ba9eb..28d98b75f 100644 --- a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs +++ b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs @@ -69,6 +69,7 @@ protected override void WindowGUI(int windowID) //GUILayout.Label(autopilot.desiredAoA.ToString()); //GUILayout.Label(autopilot.vesselState.vesselPitch.ToString()); //GUILayout.Label(autopilot.pitchCorrection.ToString()); + GUILayout.BeginHorizontal(); GUILayout.Label("pitch PID:"); autopilot.pitchPID.Kp = double.Parse(GUILayout.TextField(autopilot.pitchPID.Kp.ToString(), GUILayout.Width(40))); @@ -77,37 +78,43 @@ protected override void WindowGUI(int windowID) GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("pitch correction PID:"); - autopilot.pitchCorrectionPID.Kp = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPID.Kp.ToString(), GUILayout.Width(40))); + autopilot.pitchCorrectionPidKp = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPidKp.ToString(), GUILayout.Width(40))); autopilot.pitchCorrectionPID.Ki = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPID.Ki.ToString(), GUILayout.Width(40))); - autopilot.pitchCorrectionPID.Kd = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPID.Kd.ToString(), GUILayout.Width(40))); + autopilot.lowPitchCorPidKd = double.Parse(GUILayout.TextField(autopilot.lowPitchCorPidKd.ToString(), GUILayout.Width(30))); + autopilot.highPitchCorPidKd = double.Parse(GUILayout.TextField(autopilot.highPitchCorPidKd.ToString(), GUILayout.Width(30))); GUILayout.EndHorizontal(); GUILayout.Label(autopilot.desiredRoll.ToString()); GUILayout.Label(autopilot.autolandHeadingState.ToString()); GUILayout.Label(autopilot.velocityHeading.ToString()); GUILayout.Label(autopilot.velocityHeadingTest.ToString()); + GUILayout.Label(autopilot.rollLowPass.lastResult.ToString()); + GUILayout.Label(autopilot.desiredRollLowPass.lastResult.ToString()); + GUILayout.Label(autopilot.rollCorrectionLowPass.lastResult.ToString()); //GUILayout.Label(autopilot.vesselState.vesselRoll.ToString()); //GUILayout.Label(autopilot.rollCorrection.ToString()); GUILayout.BeginHorizontal(); GUILayout.Label("roll PID:"); - autopilot.rollPidKp = double.Parse(GUILayout.TextField(autopilot.rollPidKp.ToString(), GUILayout.Width(40))); + autopilot.rollPID.Kp = double.Parse(GUILayout.TextField(autopilot.rollPID.Kp.ToString(), GUILayout.Width(40))); autopilot.rollPID.Ki = double.Parse(GUILayout.TextField(autopilot.rollPID.Ki.ToString(), GUILayout.Width(40))); - autopilot.rollPidKd = double.Parse(GUILayout.TextField(autopilot.rollPidKd.ToString(), GUILayout.Width(40))); + autopilot.rollPID.Kd = double.Parse(GUILayout.TextField(autopilot.rollPID.Kd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("roll correction PID:"); - autopilot.rollCorrectionPID.Kp = double.Parse(GUILayout.TextField(autopilot.rollCorrectionPID.Kp.ToString(), GUILayout.Width(40))); + autopilot.rollCorrectionPidKp = double.Parse(GUILayout.TextField(autopilot.rollCorrectionPidKp.ToString(), GUILayout.Width(40))); autopilot.rollCorrectionPID.Ki = double.Parse(GUILayout.TextField(autopilot.rollCorrectionPID.Ki.ToString(), GUILayout.Width(40))); - autopilot.rollCorrectionPID.Kd = double.Parse(GUILayout.TextField(autopilot.rollCorrectionPID.Kd.ToString(), GUILayout.Width(40))); + autopilot.lowRollCorPidKd = double.Parse(GUILayout.TextField(autopilot.lowRollCorPidKd.ToString(), GUILayout.Width(30))); + autopilot.highRollCorPidKd = double.Parse(GUILayout.TextField(autopilot.highRollCorPidKd.ToString(), GUILayout.Width(30))); GUILayout.EndHorizontal(); GUILayout.Label(autopilot.desiredYaw.ToString()); GUILayout.Label(autopilot.noseYaw.ToString()); GUILayout.BeginHorizontal(); GUILayout.Label("yaw correction PID:"); - autopilot.yawCorrectionPID.Kp = double.Parse(GUILayout.TextField(autopilot.yawCorrectionPID.Kp.ToString(), GUILayout.Width(40))); + autopilot.yawCorrectionPidKp = double.Parse(GUILayout.TextField(autopilot.yawCorrectionPidKp.ToString(), GUILayout.Width(40))); autopilot.yawCorrectionPID.Ki = double.Parse(GUILayout.TextField(autopilot.yawCorrectionPID.Ki.ToString(), GUILayout.Width(40))); - autopilot.yawCorrectionPID.Kd = double.Parse(GUILayout.TextField(autopilot.yawCorrectionPID.Kd.ToString(), GUILayout.Width(40))); + autopilot.lowYawCorPidKd = double.Parse(GUILayout.TextField(autopilot.lowYawCorPidKd.ToString(), GUILayout.Width(30))); + autopilot.highYawCorPidKd = double.Parse(GUILayout.TextField(autopilot.highYawCorPidKd.ToString(), GUILayout.Width(30))); GUILayout.EndHorizontal(); //GUILayout.Label(autopilot.pitchPID.intAccum.ToString()); From 1e6de2eb4f823a15875552ba0b6410b9dc40c64b Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Fri, 5 Jul 2013 00:06:32 -0700 Subject: [PATCH 26/35] Changes to low pass filters based on vessel mass --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 17 +++++++++-------- MechJeb2/MechJebModuleSpaceplaneGuidance.cs | 11 ++--------- 2 files changed, 11 insertions(+), 17 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 5b6ea766b..fb701829b 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -38,14 +38,15 @@ public void HoldHeadingAndAltitude(object controller) public void AutopilotOn() { autopilotOn = true; - rollLowPass.initialize(); - desiredRollLowPass.initialize(); - rollCorrectionLowPass.initialize(); - yawCorrectionLowPass.initialize(); - velocityPitchLowPass.initialize(); - desiredAoALowPass.initialize(); - headingLowPass.initialize(); - velocityHeadingLowPass.initialize(); + double timeConst = 0.35 + vesselState.mass / 50; + rollLowPass.TimeConstant = timeConst * 0.15; + desiredRollLowPass.TimeConstant = timeConst * 0.1; + rollCorrectionLowPass.TimeConstant = timeConst * 0.1; + yawCorrectionLowPass.TimeConstant = timeConst * 0.1; + velocityPitchLowPass.TimeConstant = timeConst * 0.15; + desiredAoALowPass.TimeConstant = timeConst * 0.1; + headingLowPass.TimeConstant = timeConst * 0.15; + velocityHeadingLowPass.TimeConstant = timeConst * 0.15; } public void AutopilotOff() diff --git a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs index 28d98b75f..697f57437 100644 --- a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs +++ b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs @@ -84,13 +84,6 @@ protected override void WindowGUI(int windowID) autopilot.highPitchCorPidKd = double.Parse(GUILayout.TextField(autopilot.highPitchCorPidKd.ToString(), GUILayout.Width(30))); GUILayout.EndHorizontal(); - GUILayout.Label(autopilot.desiredRoll.ToString()); - GUILayout.Label(autopilot.autolandHeadingState.ToString()); - GUILayout.Label(autopilot.velocityHeading.ToString()); - GUILayout.Label(autopilot.velocityHeadingTest.ToString()); - GUILayout.Label(autopilot.rollLowPass.lastResult.ToString()); - GUILayout.Label(autopilot.desiredRollLowPass.lastResult.ToString()); - GUILayout.Label(autopilot.rollCorrectionLowPass.lastResult.ToString()); //GUILayout.Label(autopilot.vesselState.vesselRoll.ToString()); //GUILayout.Label(autopilot.rollCorrection.ToString()); GUILayout.BeginHorizontal(); @@ -107,8 +100,8 @@ protected override void WindowGUI(int windowID) autopilot.highRollCorPidKd = double.Parse(GUILayout.TextField(autopilot.highRollCorPidKd.ToString(), GUILayout.Width(30))); GUILayout.EndHorizontal(); - GUILayout.Label(autopilot.desiredYaw.ToString()); - GUILayout.Label(autopilot.noseYaw.ToString()); + //GUILayout.Label(autopilot.desiredYaw.ToString()); + //GUILayout.Label(autopilot.noseYaw.ToString()); GUILayout.BeginHorizontal(); GUILayout.Label("yaw correction PID:"); autopilot.yawCorrectionPidKp = double.Parse(GUILayout.TextField(autopilot.yawCorrectionPidKp.ToString(), GUILayout.Width(40))); From db1931dd59c651e739f68f87eff32761b199172b Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Fri, 5 Jul 2013 19:03:54 -0700 Subject: [PATCH 27/35] Implemented PID autotuning Tunes the PID controllers using estimated torques available from the control surfaces and the moment of inertia of the plane. --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 86 +++++++++++++++----- MechJeb2/MechJebModuleSpaceplaneGuidance.cs | 35 +++++--- 2 files changed, 86 insertions(+), 35 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index fb701829b..547021558 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -291,47 +291,89 @@ public void DriveAutoland(FlightCtrlState s) } } - public PIDController pitchPID = new PIDController(0.6, 0.2, 0.6, 25, -10); + public PIDController pitchPID = new PIDController(5, 0, 5, 25, -10); public PIDController pitchCorrectionPID = new PIDController(0.1, 0.05, 0.1, 1, -1); public PIDController rollPID = new PIDController(10, 0, 5); public PIDController rollCorrectionPID = new PIDController(0.01, 0, 0.01, 1, -1); public PIDController yawCorrectionPID = new PIDController(0.1, 0, 0.1, 1, -1); double altitudePercent = 0; - public double pitchCorrectionPidKp = 0.2; - public double lowPitchCorPidKd = 0.2; - public double highPitchCorPidKd = 1; - public double rollCorrectionPidKp = 0.005; - public double lowRollCorPidKd = 0.005; - public double highRollCorPidKd = 0.025; + public double pitchCorPidKp = 0.005; + public double pitchCorPidKi = 0.05; + public double pitchCorPidKd = 0.01; - public double yawCorrectionPidKp = 0.05; - public double lowYawCorPidKd = 0.05; - public double highYawCorPidKd = 0.1; + public double rollCorPidKp = 0.005; + public double rollCorPidKi = 0; + public double rollCorPidKd = 0.01; + + public double yawCorPidKp = 0.005; + public double yawCorPidKi = 0; + public double yawCorPidKd = 0.01; public double lowPitchPidMax = 25; public double highPitchPidMax = 5; public double lowPitchPidMin = -10; public double highPitchPidMin = 0; - double controlGain = 1; + + public Vector3 torqueAvailable; + public Vector3 torqueFlapsAvailable; + public Vector3 csTorqueFlaps; + public ControlSurface contSurf = new ControlSurface(); + public int contSurfNum = 0; void setPIDAltitude() { - altitudePercent = vessel.altitude / CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody); + torqueFlapsAvailable = new Vector3d(); + int contSurfCount = 0; + foreach (Part p in vessel.parts) + { + if (p is ControlSurface) + { + //find control contribution of control surface + ControlSurface cs = (p as ControlSurface); + Vector3 offset = cs.orgPos - vessel.findLocalCenterOfMass(); + Vector3 orientation = cs.orgRot.eulerAngles; + + float pitchEffectiveness = Mathf.Abs(Mathf.Cos(orientation.y * Mathf.PI / 180) * Mathf.Cos(orientation.z * Mathf.PI / 180)); + float tempTorquePitchFlaps = Mathf.Abs(pitchEffectiveness * offset.y * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * (float)vesselState.speedSurface * (float)vesselState.atmosphericDensity); + + float positionAngle = Mathf.Acos(offset.z / offset.x); + float rollEffectiveness = Mathf.Cos((orientation.y - positionAngle) * Mathf.PI / 180) * Mathf.Cos(orientation.z * Mathf.PI / 180); + float tempTorqueRollFlaps = Mathf.Abs(rollEffectiveness * Mathf.Sqrt(offset.x * offset.x + offset.z * offset.z) * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * (float)vesselState.speedSurface * (float)vesselState.atmosphericDensity); - controlGain = vesselState.mass / 50; + float yawEffectiveness = Mathf.Abs(Mathf.Sin(orientation.y * Mathf.PI / 180) * Mathf.Cos(orientation.z * Mathf.PI / 180)); + float tempTorqueYawFlaps = Mathf.Abs(yawEffectiveness * offset.y * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * (float)vesselState.speedSurface * (float)vesselState.atmosphericDensity); + + if (contSurfCount == contSurfNum) + { + contSurf = cs; + csTorqueFlaps.x = tempTorquePitchFlaps * 0.005F; + csTorqueFlaps.y = tempTorqueRollFlaps * 0.005F; + csTorqueFlaps.z = tempTorqueYawFlaps * 0.005F; + } + contSurfCount++; + + //flap torques are ESTIMATES + torqueFlapsAvailable.x += tempTorquePitchFlaps * 0.005F; + torqueFlapsAvailable.y += tempTorqueRollFlaps * 0.005F; + torqueFlapsAvailable.z += tempTorqueYawFlaps * 0.005F; + } + } - pitchCorrectionPID.Kd = lowPitchCorPidKd + (highPitchCorPidKd - lowPitchCorPidKd) * altitudePercent; - rollCorrectionPID.Kd = lowRollCorPidKd + (highRollCorPidKd - lowRollCorPidKd) * altitudePercent; - yawCorrectionPID.Kd = lowYawCorPidKd + (highYawCorPidKd - lowYawCorPidKd) * altitudePercent; + torqueAvailable.x = torqueFlapsAvailable.x + (float)vesselState.torqueThrustPYAvailable; + torqueAvailable.y = torqueFlapsAvailable.y; + torqueAvailable.z = torqueFlapsAvailable.z + (float)vesselState.torqueThrustPYAvailable; - pitchCorrectionPID.Kp = pitchCorrectionPidKp * controlGain; - pitchCorrectionPID.Kd *= controlGain; + pitchCorrectionPID.Kp = (vesselState.MoI.x / torqueAvailable.x) * pitchCorPidKp; + pitchCorrectionPID.Ki = (vesselState.MoI.x / torqueAvailable.x) * pitchCorPidKi; + pitchCorrectionPID.Kd = (vesselState.MoI.x / torqueAvailable.x) * pitchCorPidKd; - rollCorrectionPID.Kp = rollCorrectionPidKp * controlGain; - rollCorrectionPID.Kd *= controlGain; + rollCorrectionPID.Kp = (vesselState.MoI.y / torqueAvailable.y) * rollCorPidKp; + rollCorrectionPID.Ki = (vesselState.MoI.y / torqueAvailable.y) * rollCorPidKi; + rollCorrectionPID.Kd = (vesselState.MoI.y / torqueAvailable.y) * rollCorPidKd; - yawCorrectionPID.Kp = yawCorrectionPidKp * controlGain; - yawCorrectionPID.Kd *= controlGain; + yawCorrectionPID.Kp = (vesselState.MoI.z / torqueAvailable.z) * yawCorPidKp; + yawCorrectionPID.Ki = (vesselState.MoI.z / torqueAvailable.z) * yawCorPidKi; + yawCorrectionPID.Kd = (vesselState.MoI.z / torqueAvailable.z) * yawCorPidKd; pitchPID.max = lowPitchPidMax + (highPitchPidMax - lowPitchPidMax) * altitudePercent; pitchPID.min = lowPitchPidMin + (highPitchPidMin - lowPitchPidMin) * altitudePercent; diff --git a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs index 697f57437..ef165bb59 100644 --- a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs +++ b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs @@ -78,10 +78,9 @@ protected override void WindowGUI(int windowID) GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("pitch correction PID:"); - autopilot.pitchCorrectionPidKp = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPidKp.ToString(), GUILayout.Width(40))); - autopilot.pitchCorrectionPID.Ki = double.Parse(GUILayout.TextField(autopilot.pitchCorrectionPID.Ki.ToString(), GUILayout.Width(40))); - autopilot.lowPitchCorPidKd = double.Parse(GUILayout.TextField(autopilot.lowPitchCorPidKd.ToString(), GUILayout.Width(30))); - autopilot.highPitchCorPidKd = double.Parse(GUILayout.TextField(autopilot.highPitchCorPidKd.ToString(), GUILayout.Width(30))); + autopilot.pitchCorPidKp = double.Parse(GUILayout.TextField(autopilot.pitchCorPidKp.ToString(), GUILayout.Width(40))); + autopilot.pitchCorPidKi = double.Parse(GUILayout.TextField(autopilot.pitchCorPidKi.ToString(), GUILayout.Width(40))); + autopilot.pitchCorPidKd = double.Parse(GUILayout.TextField(autopilot.pitchCorPidKd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); //GUILayout.Label(autopilot.vesselState.vesselRoll.ToString()); @@ -94,27 +93,37 @@ protected override void WindowGUI(int windowID) GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("roll correction PID:"); - autopilot.rollCorrectionPidKp = double.Parse(GUILayout.TextField(autopilot.rollCorrectionPidKp.ToString(), GUILayout.Width(40))); - autopilot.rollCorrectionPID.Ki = double.Parse(GUILayout.TextField(autopilot.rollCorrectionPID.Ki.ToString(), GUILayout.Width(40))); - autopilot.lowRollCorPidKd = double.Parse(GUILayout.TextField(autopilot.lowRollCorPidKd.ToString(), GUILayout.Width(30))); - autopilot.highRollCorPidKd = double.Parse(GUILayout.TextField(autopilot.highRollCorPidKd.ToString(), GUILayout.Width(30))); + autopilot.rollCorPidKp = double.Parse(GUILayout.TextField(autopilot.rollCorPidKp.ToString(), GUILayout.Width(40))); + autopilot.rollCorPidKi = double.Parse(GUILayout.TextField(autopilot.rollCorPidKi.ToString(), GUILayout.Width(40))); + autopilot.rollCorPidKd = double.Parse(GUILayout.TextField(autopilot.rollCorPidKd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); //GUILayout.Label(autopilot.desiredYaw.ToString()); //GUILayout.Label(autopilot.noseYaw.ToString()); GUILayout.BeginHorizontal(); GUILayout.Label("yaw correction PID:"); - autopilot.yawCorrectionPidKp = double.Parse(GUILayout.TextField(autopilot.yawCorrectionPidKp.ToString(), GUILayout.Width(40))); - autopilot.yawCorrectionPID.Ki = double.Parse(GUILayout.TextField(autopilot.yawCorrectionPID.Ki.ToString(), GUILayout.Width(40))); - autopilot.lowYawCorPidKd = double.Parse(GUILayout.TextField(autopilot.lowYawCorPidKd.ToString(), GUILayout.Width(30))); - autopilot.highYawCorPidKd = double.Parse(GUILayout.TextField(autopilot.highYawCorPidKd.ToString(), GUILayout.Width(30))); + autopilot.yawCorPidKp = double.Parse(GUILayout.TextField(autopilot.yawCorPidKp.ToString(), GUILayout.Width(40))); + autopilot.yawCorPidKi = double.Parse(GUILayout.TextField(autopilot.yawCorPidKi.ToString(), GUILayout.Width(40))); + autopilot.yawCorPidKd = double.Parse(GUILayout.TextField(autopilot.yawCorPidKd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); //GUILayout.Label(autopilot.pitchPID.intAccum.ToString()); //GUILayout.Label(autopilot.pitchCorrectionPID.intAccum.ToString()); //GUILayout.Label(autopilot.rollPID.intAccum.ToString()); //GUILayout.Label(autopilot.rollCorrectionPID.intAccum.ToString()); - + /* + autopilot.contSurfNum = int.Parse(GUILayout.TextField(autopilot.contSurfNum.ToString(), GUILayout.Width(40))); + GUILayout.Label(autopilot.vessel.findLocalCenterOfMass().ToString()); + GUILayout.Label((autopilot.contSurf.orgPos - autopilot.vessel.findLocalCenterOfMass()).ToString()); + GUILayout.Label(autopilot.contSurf.orgRot.eulerAngles.ToString()); + GUILayout.Label(autopilot.contSurf.ctrlSurfaceArea.ToString()); + GUILayout.Label(autopilot.contSurf.ctrlSurfaceRange.ToString()); + GUILayout.Label(autopilot.contSurf.deflectionLiftCoeff.ToString()); + GUILayout.Label(autopilot.csTorqueFlaps.ToString()); + GUILayout.Label(autopilot.torqueFlapsAvailable.ToString()); + GUILayout.Label(autopilot.torqueAvailable.ToString()); + GUILayout.Label(autopilot.vesselState.MoI.ToString()); + */ if (autopilot.enabled && autopilot.mode == MechJebModuleSpaceplaneAutopilot.Mode.HOLD && GUILayout.Button("Abort")) autopilot.Abort(); From 9c75172d0f4b933bdd9250edcdb44ec14b9aad42 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Sat, 6 Jul 2013 00:55:01 -0700 Subject: [PATCH 28/35] PID autotune tweaks --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 55 ++++++++++++++------ MechJeb2/MechJebModuleSpaceplaneGuidance.cs | 49 ++++++++++++----- 2 files changed, 76 insertions(+), 28 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 547021558..bbe354f32 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -112,7 +112,7 @@ public override void Drive(FlightCtrlState s) } } - public float takeoffPitch = 0.5F; + public float takeoffPitch = 10F; public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { if (!part.vessel.Landed) @@ -141,6 +141,8 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { //vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, false); loweredGear = true; + pitchCorrectionPID.intAccum = 0; + pitchPID.intAccum = 0; landed = false; } } @@ -169,7 +171,7 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) //pitch up for takeoff if (!brakes && vesselState.time > landTime + 1.0) { - vessel.ctrlState.pitch = takeoffPitch; + vessel.ctrlState.pitch = (float)(takeoffPitch - vesselState.vesselPitch) / 15; } vessel.ctrlState.yaw = (float)MuUtils.ClampDegrees180(targetHeading - vesselState.vesselHeading) * 0.1F; } @@ -291,24 +293,32 @@ public void DriveAutoland(FlightCtrlState s) } } - public PIDController pitchPID = new PIDController(5, 0, 5, 25, -10); - public PIDController pitchCorrectionPID = new PIDController(0.1, 0.05, 0.1, 1, -1); - public PIDController rollPID = new PIDController(10, 0, 5); + public PIDController pitchPID = new PIDController(2, 0.7, 2, 25, -10); + public PIDController pitchCorrectionPID = new PIDController(0.1, 0.01, 0.1, 1, -1); + public PIDController rollPID = new PIDController(2, 0, 2); public PIDController rollCorrectionPID = new PIDController(0.01, 0, 0.01, 1, -1); public PIDController yawCorrectionPID = new PIDController(0.1, 0, 0.1, 1, -1); double altitudePercent = 0; public double pitchCorPidKp = 0.005; - public double pitchCorPidKi = 0.05; - public double pitchCorPidKd = 0.01; + public double pitchCorPidKi = 0.01; + public double pitchCorPidKd = 0.005; + + public double pitchPidKp = 2; + public double pitchPidKi = 0.7; + public double pitchPidKd = 2; public double rollCorPidKp = 0.005; public double rollCorPidKi = 0; - public double rollCorPidKd = 0.01; + public double rollCorPidKd = 0.005; + + public double rollPidKp = 2; + public double rollPidKi = 0; + public double rollPidKd = 2; public double yawCorPidKp = 0.005; public double yawCorPidKi = 0; - public double yawCorPidKd = 0.01; + public double yawCorPidKd = 0.005; public double lowPitchPidMax = 25; public double highPitchPidMax = 5; @@ -332,16 +342,19 @@ void setPIDAltitude() ControlSurface cs = (p as ControlSurface); Vector3 offset = cs.orgPos - vessel.findLocalCenterOfMass(); Vector3 orientation = cs.orgRot.eulerAngles; + float speed = Mathf.Sqrt((float)vesselState.speedSurface * (float)vesselState.speedSurface + (float)vesselState.speedVertical * (float)vesselState.speedVertical); float pitchEffectiveness = Mathf.Abs(Mathf.Cos(orientation.y * Mathf.PI / 180) * Mathf.Cos(orientation.z * Mathf.PI / 180)); - float tempTorquePitchFlaps = Mathf.Abs(pitchEffectiveness * offset.y * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * (float)vesselState.speedSurface * (float)vesselState.atmosphericDensity); + float tempTorquePitchFlaps = Mathf.Abs(pitchEffectiveness * offset.y * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * speed * (float)vesselState.atmosphericDensity); float positionAngle = Mathf.Acos(offset.z / offset.x); + if (float.IsNaN(positionAngle)) positionAngle = 90; float rollEffectiveness = Mathf.Cos((orientation.y - positionAngle) * Mathf.PI / 180) * Mathf.Cos(orientation.z * Mathf.PI / 180); - float tempTorqueRollFlaps = Mathf.Abs(rollEffectiveness * Mathf.Sqrt(offset.x * offset.x + offset.z * offset.z) * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * (float)vesselState.speedSurface * (float)vesselState.atmosphericDensity); + if (float.IsNaN(rollEffectiveness)) rollEffectiveness = 1; + float tempTorqueRollFlaps = Mathf.Abs(rollEffectiveness * Mathf.Sqrt(offset.x * offset.x + offset.z * offset.z) * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * speed * (float)vesselState.atmosphericDensity); float yawEffectiveness = Mathf.Abs(Mathf.Sin(orientation.y * Mathf.PI / 180) * Mathf.Cos(orientation.z * Mathf.PI / 180)); - float tempTorqueYawFlaps = Mathf.Abs(yawEffectiveness * offset.y * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * (float)vesselState.speedSurface * (float)vesselState.atmosphericDensity); + float tempTorqueYawFlaps = Mathf.Abs(yawEffectiveness * offset.y * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * speed * (float)vesselState.atmosphericDensity); if (contSurfCount == contSurfNum) { @@ -367,10 +380,18 @@ void setPIDAltitude() pitchCorrectionPID.Ki = (vesselState.MoI.x / torqueAvailable.x) * pitchCorPidKi; pitchCorrectionPID.Kd = (vesselState.MoI.x / torqueAvailable.x) * pitchCorPidKd; + pitchPID.Kp = pitchPidKp;// / vesselState.MoI.x; + pitchPID.Ki = pitchPidKi;// / vesselState.MoI.x; + pitchPID.Kd = pitchPidKd;// / vesselState.MoI.x; + rollCorrectionPID.Kp = (vesselState.MoI.y / torqueAvailable.y) * rollCorPidKp; rollCorrectionPID.Ki = (vesselState.MoI.y / torqueAvailable.y) * rollCorPidKi; rollCorrectionPID.Kd = (vesselState.MoI.y / torqueAvailable.y) * rollCorPidKd; + rollPID.Kp = rollPidKp;// / vesselState.MoI.x; + rollPID.Ki = rollPidKi;// / vesselState.MoI.x; + rollPID.Kd = rollPidKd;// / vesselState.MoI.x; + yawCorrectionPID.Kp = (vesselState.MoI.z / torqueAvailable.z) * yawCorPidKp; yawCorrectionPID.Ki = (vesselState.MoI.z / torqueAvailable.z) * yawCorPidKi; yawCorrectionPID.Kd = (vesselState.MoI.z / torqueAvailable.z) * yawCorPidKd; @@ -406,6 +427,8 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) if (autopilotOn) { setPIDAltitude(); + + desiredFpa = MuUtils.Clamp(desiredFpa, -20, 20); velocityHeading = velocityHeadingLowPass.calc(180 / Math.PI * Math.Atan2(Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.east), Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.north))); @@ -427,10 +450,10 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) vessel.ctrlState.pitch = (float)pitchCorrection; vessel.ctrlState.yaw = (float)yawCorrection; - if (pitchPID.intAccum > 100) pitchPID.intAccum = 100; - if (pitchPID.intAccum < -100) pitchPID.intAccum = -100; - if (pitchCorrectionPID.intAccum > 10) pitchCorrectionPID.intAccum = 10; - if (pitchCorrectionPID.intAccum < -10) pitchCorrectionPID.intAccum = -10; + if (pitchPID.intAccum > 1000) pitchPID.intAccum = 1000; + if (pitchPID.intAccum < -1000) pitchPID.intAccum = -1000; + if (pitchCorrectionPID.intAccum > 100) pitchCorrectionPID.intAccum = 100; + if (pitchCorrectionPID.intAccum < -100) pitchCorrectionPID.intAccum = -100; //if (rollPID.intAccum > 100) rollPID.intAccum = 100; //if (rollPID.intAccum < -100) rollPID.intAccum = -100; //if (rollCorrectionPID.intAccum > 10) rollCorrectionPID.intAccum = 10; diff --git a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs index ef165bb59..1f5a2346e 100644 --- a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs +++ b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs @@ -72,9 +72,14 @@ protected override void WindowGUI(int windowID) GUILayout.BeginHorizontal(); GUILayout.Label("pitch PID:"); - autopilot.pitchPID.Kp = double.Parse(GUILayout.TextField(autopilot.pitchPID.Kp.ToString(), GUILayout.Width(40))); - autopilot.pitchPID.Ki = double.Parse(GUILayout.TextField(autopilot.pitchPID.Ki.ToString(), GUILayout.Width(40))); - autopilot.pitchPID.Kd = double.Parse(GUILayout.TextField(autopilot.pitchPID.Kd.ToString(), GUILayout.Width(40))); + autopilot.pitchPidKp = double.Parse(GUILayout.TextField(autopilot.pitchPidKp.ToString(), GUILayout.Width(40))); + autopilot.pitchPidKi = double.Parse(GUILayout.TextField(autopilot.pitchPidKi.ToString(), GUILayout.Width(40))); + autopilot.pitchPidKd = double.Parse(GUILayout.TextField(autopilot.pitchPidKd.ToString(), GUILayout.Width(40))); + GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); + GUILayout.Label(autopilot.pitchPID.Kp.ToString("F5")); + GUILayout.Label(autopilot.pitchPID.Ki.ToString("F5")); + GUILayout.Label(autopilot.pitchPID.Kd.ToString("F5")); GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("pitch correction PID:"); @@ -82,14 +87,24 @@ protected override void WindowGUI(int windowID) autopilot.pitchCorPidKi = double.Parse(GUILayout.TextField(autopilot.pitchCorPidKi.ToString(), GUILayout.Width(40))); autopilot.pitchCorPidKd = double.Parse(GUILayout.TextField(autopilot.pitchCorPidKd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); + GUILayout.Label(autopilot.pitchCorrectionPID.Kp.ToString("F5")); + GUILayout.Label(autopilot.pitchCorrectionPID.Ki.ToString("F5")); + GUILayout.Label(autopilot.pitchCorrectionPID.Kd.ToString("F5")); + GUILayout.EndHorizontal(); //GUILayout.Label(autopilot.vesselState.vesselRoll.ToString()); //GUILayout.Label(autopilot.rollCorrection.ToString()); GUILayout.BeginHorizontal(); GUILayout.Label("roll PID:"); - autopilot.rollPID.Kp = double.Parse(GUILayout.TextField(autopilot.rollPID.Kp.ToString(), GUILayout.Width(40))); - autopilot.rollPID.Ki = double.Parse(GUILayout.TextField(autopilot.rollPID.Ki.ToString(), GUILayout.Width(40))); - autopilot.rollPID.Kd = double.Parse(GUILayout.TextField(autopilot.rollPID.Kd.ToString(), GUILayout.Width(40))); + autopilot.rollPidKp = double.Parse(GUILayout.TextField(autopilot.rollPidKp.ToString(), GUILayout.Width(40))); + autopilot.rollPidKi = double.Parse(GUILayout.TextField(autopilot.rollPidKi.ToString(), GUILayout.Width(40))); + autopilot.rollPidKd = double.Parse(GUILayout.TextField(autopilot.rollPidKd.ToString(), GUILayout.Width(40))); + GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); + GUILayout.Label(autopilot.rollPID.Kp.ToString("F5")); + GUILayout.Label(autopilot.rollPID.Ki.ToString("F5")); + GUILayout.Label(autopilot.rollPID.Kd.ToString("F5")); GUILayout.EndHorizontal(); GUILayout.BeginHorizontal(); GUILayout.Label("roll correction PID:"); @@ -97,6 +112,11 @@ protected override void WindowGUI(int windowID) autopilot.rollCorPidKi = double.Parse(GUILayout.TextField(autopilot.rollCorPidKi.ToString(), GUILayout.Width(40))); autopilot.rollCorPidKd = double.Parse(GUILayout.TextField(autopilot.rollCorPidKd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); + GUILayout.Label(autopilot.rollCorrectionPID.Kp.ToString("F5")); + GUILayout.Label(autopilot.rollCorrectionPID.Ki.ToString("F5")); + GUILayout.Label(autopilot.rollCorrectionPID.Kd.ToString("F5")); + GUILayout.EndHorizontal(); //GUILayout.Label(autopilot.desiredYaw.ToString()); //GUILayout.Label(autopilot.noseYaw.ToString()); @@ -106,24 +126,29 @@ protected override void WindowGUI(int windowID) autopilot.yawCorPidKi = double.Parse(GUILayout.TextField(autopilot.yawCorPidKi.ToString(), GUILayout.Width(40))); autopilot.yawCorPidKd = double.Parse(GUILayout.TextField(autopilot.yawCorPidKd.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); + GUILayout.Label(autopilot.yawCorrectionPID.Kp.ToString("F5")); + GUILayout.Label(autopilot.yawCorrectionPID.Ki.ToString("F5")); + GUILayout.Label(autopilot.yawCorrectionPID.Kd.ToString("F5")); + GUILayout.EndHorizontal(); //GUILayout.Label(autopilot.pitchPID.intAccum.ToString()); //GUILayout.Label(autopilot.pitchCorrectionPID.intAccum.ToString()); //GUILayout.Label(autopilot.rollPID.intAccum.ToString()); //GUILayout.Label(autopilot.rollCorrectionPID.intAccum.ToString()); - /* + autopilot.contSurfNum = int.Parse(GUILayout.TextField(autopilot.contSurfNum.ToString(), GUILayout.Width(40))); GUILayout.Label(autopilot.vessel.findLocalCenterOfMass().ToString()); GUILayout.Label((autopilot.contSurf.orgPos - autopilot.vessel.findLocalCenterOfMass()).ToString()); GUILayout.Label(autopilot.contSurf.orgRot.eulerAngles.ToString()); - GUILayout.Label(autopilot.contSurf.ctrlSurfaceArea.ToString()); - GUILayout.Label(autopilot.contSurf.ctrlSurfaceRange.ToString()); - GUILayout.Label(autopilot.contSurf.deflectionLiftCoeff.ToString()); + //GUILayout.Label(autopilot.contSurf.ctrlSurfaceArea.ToString()); + //GUILayout.Label(autopilot.contSurf.ctrlSurfaceRange.ToString()); + //GUILayout.Label(autopilot.contSurf.deflectionLiftCoeff.ToString()); GUILayout.Label(autopilot.csTorqueFlaps.ToString()); GUILayout.Label(autopilot.torqueFlapsAvailable.ToString()); GUILayout.Label(autopilot.torqueAvailable.ToString()); - GUILayout.Label(autopilot.vesselState.MoI.ToString()); - */ + GUILayout.Label(((Vector3d)autopilot.vesselState.MoI).ToString()); + if (autopilot.enabled && autopilot.mode == MechJebModuleSpaceplaneAutopilot.Mode.HOLD && GUILayout.Button("Abort")) autopilot.Abort(); From e89214c05803f5c88dcdda094a25be1d442f4b64 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Sat, 6 Jul 2013 19:19:53 -0700 Subject: [PATCH 29/35] PID autotune equation correction --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 52 ++++++++++---------- 1 file changed, 26 insertions(+), 26 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index bbe354f32..5b474d33f 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -38,7 +38,7 @@ public void HoldHeadingAndAltitude(object controller) public void AutopilotOn() { autopilotOn = true; - double timeConst = 0.35 + vesselState.mass / 50; + double timeConst = 0.4;// +vesselState.mass / 50; rollLowPass.TimeConstant = timeConst * 0.15; desiredRollLowPass.TimeConstant = timeConst * 0.1; rollCorrectionLowPass.TimeConstant = timeConst * 0.1; @@ -293,32 +293,32 @@ public void DriveAutoland(FlightCtrlState s) } } - public PIDController pitchPID = new PIDController(2, 0.7, 2, 25, -10); + public PIDController pitchPID = new PIDController(2, 0.7, 2, 20, -10); public PIDController pitchCorrectionPID = new PIDController(0.1, 0.01, 0.1, 1, -1); public PIDController rollPID = new PIDController(2, 0, 2); public PIDController rollCorrectionPID = new PIDController(0.01, 0, 0.01, 1, -1); public PIDController yawCorrectionPID = new PIDController(0.1, 0, 0.1, 1, -1); double altitudePercent = 0; - public double pitchCorPidKp = 0.005; + public double pitchCorPidKp = 0.01; public double pitchCorPidKi = 0.01; - public double pitchCorPidKd = 0.005; + public double pitchCorPidKd = 0.01; - public double pitchPidKp = 2; - public double pitchPidKi = 0.7; - public double pitchPidKd = 2; + public double pitchPidKp = 4; + public double pitchPidKi = 1; + public double pitchPidKd = 4; - public double rollCorPidKp = 0.005; + public double rollCorPidKp = 0.01; public double rollCorPidKi = 0; - public double rollCorPidKd = 0.005; + public double rollCorPidKd = 0.01; - public double rollPidKp = 2; + public double rollPidKp = 4; public double rollPidKi = 0; - public double rollPidKd = 2; + public double rollPidKd = 4; - public double yawCorPidKp = 0.005; + public double yawCorPidKp = 0.01; public double yawCorPidKi = 0; - public double yawCorPidKd = 0.005; + public double yawCorPidKd = 0.01; public double lowPitchPidMax = 25; public double highPitchPidMax = 5; @@ -345,30 +345,30 @@ void setPIDAltitude() float speed = Mathf.Sqrt((float)vesselState.speedSurface * (float)vesselState.speedSurface + (float)vesselState.speedVertical * (float)vesselState.speedVertical); float pitchEffectiveness = Mathf.Abs(Mathf.Cos(orientation.y * Mathf.PI / 180) * Mathf.Cos(orientation.z * Mathf.PI / 180)); - float tempTorquePitchFlaps = Mathf.Abs(pitchEffectiveness * offset.y * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * speed * (float)vesselState.atmosphericDensity); + float tempTorquePitchFlaps = Mathf.Abs(pitchEffectiveness * offset.y * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * speed * speed * (float)vesselState.atmosphericDensity); float positionAngle = Mathf.Acos(offset.z / offset.x); if (float.IsNaN(positionAngle)) positionAngle = 90; float rollEffectiveness = Mathf.Cos((orientation.y - positionAngle) * Mathf.PI / 180) * Mathf.Cos(orientation.z * Mathf.PI / 180); if (float.IsNaN(rollEffectiveness)) rollEffectiveness = 1; - float tempTorqueRollFlaps = Mathf.Abs(rollEffectiveness * Mathf.Sqrt(offset.x * offset.x + offset.z * offset.z) * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * speed * (float)vesselState.atmosphericDensity); + float tempTorqueRollFlaps = Mathf.Abs(rollEffectiveness * Mathf.Sqrt(offset.x * offset.x + offset.z * offset.z) * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * speed * speed * (float)vesselState.atmosphericDensity); float yawEffectiveness = Mathf.Abs(Mathf.Sin(orientation.y * Mathf.PI / 180) * Mathf.Cos(orientation.z * Mathf.PI / 180)); - float tempTorqueYawFlaps = Mathf.Abs(yawEffectiveness * offset.y * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * speed * (float)vesselState.atmosphericDensity); + float tempTorqueYawFlaps = Mathf.Abs(yawEffectiveness * offset.y * cs.ctrlSurfaceArea * cs.ctrlSurfaceRange * speed * speed * (float)vesselState.atmosphericDensity); if (contSurfCount == contSurfNum) { contSurf = cs; - csTorqueFlaps.x = tempTorquePitchFlaps * 0.005F; - csTorqueFlaps.y = tempTorqueRollFlaps * 0.005F; - csTorqueFlaps.z = tempTorqueYawFlaps * 0.005F; + csTorqueFlaps.x = tempTorquePitchFlaps * 0.0001F; + csTorqueFlaps.y = tempTorqueRollFlaps * 0.0001F; + csTorqueFlaps.z = tempTorqueYawFlaps * 0.0001F; } contSurfCount++; //flap torques are ESTIMATES - torqueFlapsAvailable.x += tempTorquePitchFlaps * 0.005F; - torqueFlapsAvailable.y += tempTorqueRollFlaps * 0.005F; - torqueFlapsAvailable.z += tempTorqueYawFlaps * 0.005F; + torqueFlapsAvailable.x += tempTorquePitchFlaps * 0.0001F; + torqueFlapsAvailable.y += tempTorqueRollFlaps * 0.0001F; + torqueFlapsAvailable.z += tempTorqueYawFlaps * 0.0001F; } } @@ -450,10 +450,10 @@ void AimVelocityVector(double desiredFpa, double desiredHeading) vessel.ctrlState.pitch = (float)pitchCorrection; vessel.ctrlState.yaw = (float)yawCorrection; - if (pitchPID.intAccum > 1000) pitchPID.intAccum = 1000; - if (pitchPID.intAccum < -1000) pitchPID.intAccum = -1000; - if (pitchCorrectionPID.intAccum > 100) pitchCorrectionPID.intAccum = 100; - if (pitchCorrectionPID.intAccum < -100) pitchCorrectionPID.intAccum = -100; + if (pitchPID.intAccum > 10000) pitchPID.intAccum = 10000; + if (pitchPID.intAccum < -10000) pitchPID.intAccum = -10000; + if (pitchCorrectionPID.intAccum > 10000) pitchCorrectionPID.intAccum = 10000; + if (pitchCorrectionPID.intAccum < -10000) pitchCorrectionPID.intAccum = -10000; //if (rollPID.intAccum > 100) rollPID.intAccum = 100; //if (rollPID.intAccum < -100) rollPID.intAccum = -100; //if (rollCorrectionPID.intAccum > 10) rollCorrectionPID.intAccum = 10; From 54d4f8efc2d9fd8aec1de27c2e936edba42860f0 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Sat, 6 Jul 2013 19:46:59 -0700 Subject: [PATCH 30/35] Added torque from command modules into autotune --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 5b474d33f..c4e328163 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -372,9 +372,9 @@ void setPIDAltitude() } } - torqueAvailable.x = torqueFlapsAvailable.x + (float)vesselState.torqueThrustPYAvailable; - torqueAvailable.y = torqueFlapsAvailable.y; - torqueAvailable.z = torqueFlapsAvailable.z + (float)vesselState.torqueThrustPYAvailable; + torqueAvailable.x = torqueFlapsAvailable.x + (float)vesselState.torquePYAvailable + (float)vesselState.torqueThrustPYAvailable; + torqueAvailable.y = torqueFlapsAvailable.y + (float)vesselState.torqueRAvailable; + torqueAvailable.z = torqueFlapsAvailable.z + (float)vesselState.torquePYAvailable + (float)vesselState.torqueThrustPYAvailable; pitchCorrectionPID.Kp = (vesselState.MoI.x / torqueAvailable.x) * pitchCorPidKp; pitchCorrectionPID.Ki = (vesselState.MoI.x / torqueAvailable.x) * pitchCorPidKi; From 719ee9368dfe5fa3c7acd4564bfd1e473876b776 Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Sun, 7 Jul 2013 02:24:21 -0700 Subject: [PATCH 31/35] PID tweaks --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index c4e328163..85e0885da 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -300,15 +300,15 @@ public void DriveAutoland(FlightCtrlState s) public PIDController yawCorrectionPID = new PIDController(0.1, 0, 0.1, 1, -1); double altitudePercent = 0; - public double pitchCorPidKp = 0.01; - public double pitchCorPidKi = 0.01; + public double pitchCorPidKp = 0.02; + public double pitchCorPidKi = 0.005; public double pitchCorPidKd = 0.01; public double pitchPidKp = 4; public double pitchPidKi = 1; public double pitchPidKd = 4; - public double rollCorPidKp = 0.01; + public double rollCorPidKp = 0.02; public double rollCorPidKi = 0; public double rollCorPidKd = 0.01; @@ -316,7 +316,7 @@ public void DriveAutoland(FlightCtrlState s) public double rollPidKi = 0; public double rollPidKd = 4; - public double yawCorPidKp = 0.01; + public double yawCorPidKp = 0.02; public double yawCorPidKi = 0; public double yawCorPidKd = 0.01; From 802914dac1262d1266dd8579a8e765a70837d63a Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Tue, 9 Jul 2013 00:53:51 -0700 Subject: [PATCH 32/35] Spaceplane autopilot upgrades --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 230 ++++++++++++++++--- MechJeb2/MechJebModuleSpaceplaneGuidance.cs | 9 +- 2 files changed, 206 insertions(+), 33 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index aa9ac41e3..760a1299c 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -10,24 +10,39 @@ public class MechJebModuleSpaceplaneAutopilot : ComputerModule { public void Autoland(object controller) { - users.Add(controller); - core.attitude.users.Add(this); mode = Mode.AUTOLAND; + users.Add(controller); loweredGear = false; + brakes = false; } public void HoldHeadingAndAltitude(object controller) { + mode = Mode.HOLD; users.Add(controller); + loweredGear = false; + brakes = false; + } + + bool autopilotOn = false; + public void AutopilotOn() + { core.attitude.users.Add(this); - mode = Mode.HOLD; + autopilotOn = true; } public void AutopilotOff() { - mode = Mode.OFF; + core.attitude.users.Clear(); + autopilotOn = false; + } + + public void Abort() + { users.Clear(); + AutopilotOff(); core.attitude.attitudeDeactivate(); + mode = Mode.OFF; } public static Runway[] runways = @@ -59,6 +74,7 @@ public enum Mode { AUTOLAND, HOLD, OFF }; public EditableDouble targetHeading = 90; bool loweredGear = false; + bool brakes = false; public override void OnModuleDisabled() { @@ -79,19 +95,96 @@ public override void Drive(FlightCtrlState s) } } + public float takeoffPitch = 10F; public void DriveHeadingAndAltitudeHold(FlightCtrlState s) { - double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / 30.0; - double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 9), (float)Math.Sin(Math.PI / 9))); + if (!part.vessel.Landed) + { + if (!autopilotOn) + AutopilotOn(); + if (!loweredGear) + { + if ((part.vessel.altitude - part.vessel.terrainAltitude) < 100.0) + { + vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, true); + loweredGear = true; + } + } + else + { + if ((part.vessel.altitude - part.vessel.terrainAltitude) > 100.0) + { + vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, false); + loweredGear = false; + } + } + + //takeoff or set for flight + if (landed) + { + //vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, false); + loweredGear = true; + landed = false; + } + } + else + { + if (!landed) + { + vessel.ctrlState.mainThrottle = 0; + AutopilotOff(); + landTime = vesselState.time; + landed = true; + loweredGear = true; + } + //apply breaks a little after touchdown + if (!brakes && vesselState.time > landTime + 1.0) + { + vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true); + brakes = true; + } + //if engines are started, disengage brakes and prepare for takeoff + if (brakes && vessel.ctrlState.mainThrottle > 0.01) + { + vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, false); + brakes = false; + } + //pitch up for takeoff + if (!brakes && vesselState.time > landTime + 1.0) + { + vessel.ctrlState.pitch = (float)(takeoffPitch - vesselState.vesselPitch) / 15; + } + vessel.ctrlState.yaw = (float)MuUtils.ClampDegrees180(targetHeading - vesselState.vesselHeading) * 0.1F; + } + + double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) / (CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 5)); + double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 7), (float)Math.Sin(Math.PI / 7))); + + double heading = targetHeading; + //if (loweredGear == true) heading = vesselState.vesselHeading; + AimVelocityVector(targetFlightPathAngle, targetHeading); } + bool landed = false; + double landTime = 0; + public double aimAltitude = 0; + public double distanceFrom = 0; + public double runwayHeading = 0; + public enum HeadingState { RIGHT, LEFT, OFF }; + public HeadingState autolandHeadingState = HeadingState.OFF; public void DriveAutoland(FlightCtrlState s) { if (!part.vessel.Landed) { + if (landed) landed = false; + Vector3d runwayStart = RunwayStart(); + if (!autopilotOn) + AutopilotOn(); + + //prepare for landing if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 1000.0) { vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, true); @@ -100,53 +193,128 @@ public void DriveAutoland(FlightCtrlState s) Vector3d vectorToWaypoint = ILSAimDirection(); double headingToWaypoint = vesselState.HeadingFromDirection(vectorToWaypoint); + if (Math.Abs(MuUtils.ClampDegrees180(headingToWaypoint - vesselState.vesselHeading)) > 170) + { + //make sure the heading doesn't keep switching from side to side + switch (autolandHeadingState) + { + case HeadingState.RIGHT: + headingToWaypoint = MuUtils.ClampDegrees360(vesselState.vesselHeading + 90); + break; + case HeadingState.LEFT: + headingToWaypoint = MuUtils.ClampDegrees360(vesselState.vesselHeading - 90); + break; + case HeadingState.OFF: + if (headingToWaypoint - vesselState.vesselHeading > 0) + autolandHeadingState = HeadingState.RIGHT; + else + autolandHeadingState = HeadingState.LEFT; + break; + } + } + else + autolandHeadingState = HeadingState.OFF; + + //stop any rolling and aim down runway before touching down + if ((vesselState.CoM - runwayStart).magnitude < 500.0) + { + Vector3d runwayDir = runway.End(vesselState.CoM) - runway.Start(vesselState.CoM); + runwayHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(runwayDir, vesselState.east), Vector3d.Dot(runwayDir, vesselState.north)); + headingToWaypoint = runwayHeading; + } Vector3d vectorToRunway = runwayStart - vesselState.CoM; double verticalDistanceToRunway = Vector3d.Dot(vectorToRunway, vesselState.up); double horizontalDistanceToRunway = Math.Sqrt(vectorToRunway.sqrMagnitude - verticalDistanceToRunway * verticalDistanceToRunway); - double flightPathAngleToRunway = 180 / Math.PI * Math.Atan2(verticalDistanceToRunway, horizontalDistanceToRunway); - double desiredFPA = Mathf.Clamp((float)(flightPathAngleToRunway + 3 * (flightPathAngleToRunway + glideslope)), -20.0F, 0.0F); + distanceFrom = horizontalDistanceToRunway; + if (horizontalDistanceToRunway > 10000) + { + aimAltitude = 1000; + double horizontalDifference = 9900; + if (horizontalDistanceToRunway > 40000) { aimAltitude = 5000; horizontalDifference = 39900; } + if (horizontalDistanceToRunway > 80000) + { + aimAltitude = vesselState.altitudeTrue; + if (aimAltitude < 5000) aimAltitude = 5000; + horizontalDifference = 79900; + } + double setupFPA = MuUtils.Clamp(180 / Math.PI * Math.Atan2(aimAltitude - vessel.altitude, horizontalDistanceToRunway - horizontalDifference), -20, 20); + + AimVelocityVector(setupFPA, headingToWaypoint); + } + else + { + double flightPathAngleToRunway = 180 / Math.PI * Math.Atan2(verticalDistanceToRunway, horizontalDistanceToRunway); + double desiredFPA = Mathf.Clamp((float)(flightPathAngleToRunway + 3 * (flightPathAngleToRunway + glideslope)), -20.0F, 5.0F); - AimVelocityVector(desiredFPA, headingToWaypoint); + aimAltitude = verticalDistanceToRunway; + AimVelocityVector(desiredFPA, headingToWaypoint); + } } else { + if (!landed) + { + vessel.ctrlState.mainThrottle = 0; + landTime = vesselState.time; + AutopilotOff(); + landed = true; + } + //apply breaks a little after touchdown + if (!brakes && vesselState.time > landTime + 1.0) + { + vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true); + vessel.ctrlState.pitch = -1; + brakes = true; + } //keep the plane aligned with the runway: Vector3d runwayDir = runway.End(vesselState.CoM) - runway.Start(vesselState.CoM); if (Vector3d.Dot(runwayDir, vesselState.forward) < 0) runwayDir *= -1; - double runwayHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(runwayDir, vesselState.east), Vector3d.Dot(runwayDir, vesselState.north)); - core.attitude.attitudeTo(runwayHeading, 0, 0, this); + runwayHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(runwayDir, vesselState.east), Vector3d.Dot(runwayDir, vesselState.north)); + vessel.ctrlState.pitch = (float)MuUtils.Clamp(-vesselState.vesselPitch * 0.1, -0.5, 0); + vessel.ctrlState.yaw = (float)MuUtils.Clamp(MuUtils.ClampDegrees180(runwayHeading - vesselState.vesselHeading) * 0.1F, -0.1, 0.1); + if (vesselState.vesselRoll < -1) vessel.ctrlState.roll = 1; + if (vesselState.vesselRoll > 1) vessel.ctrlState.roll = -1; } } public double stableAoA = 0; //we average AoA over time to get an estimate of what pitch will produce what FPA public double pitchCorrection = 0; //we average (commanded pitch - actual pitch) over time in order to fix this offset in our commands - public float maxYaw = 10.0F; - public float maxRoll = 10.0F; + public float maxYaw = 15.0F; + public float maxRoll = 30.0F; + public float maxAoA = 30.0F; + public float minAoA = -10.0F; public float maxPitchCorrection = 5.0F; public double AoAtimeConstant = 2.0; public double pitchCorrectionTimeConstant = 15.0; + public double velocityPitch = 0; + public double percentAltitude = 0; void AimVelocityVector(double desiredFpa, double desiredHeading) { - //horizontal control - double velocityHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.east), - Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.north)); - double headingTurn = Mathf.Clamp((float)MuUtils.ClampDegrees180(desiredHeading - velocityHeading), -maxYaw, maxYaw); - double noseHeading = velocityHeading + headingTurn; - double noseRoll = (maxRoll / maxYaw) * headingTurn; - - //vertical control - double nosePitch = desiredFpa + stableAoA + pitchCorrection; - - core.attitude.attitudeTo(noseHeading, nosePitch, noseRoll, this); - - double flightPathAngle = 180 / Math.PI * Math.Atan2(vesselState.speedVertical, vesselState.speedSurfaceHorizontal); - double AoA = vesselState.vesselPitch - flightPathAngle; - stableAoA = (AoAtimeConstant * stableAoA + vesselState.deltaT * AoA) / (AoAtimeConstant + vesselState.deltaT); //a sort of integral error - - pitchCorrection = (pitchCorrectionTimeConstant * pitchCorrection + vesselState.deltaT * (nosePitch - vesselState.vesselPitch)) / (pitchCorrectionTimeConstant + vesselState.deltaT); - pitchCorrection = Mathf.Clamp((float)pitchCorrection, -maxPitchCorrection, maxPitchCorrection); + if (autopilotOn) + { + //horizontal control + double velocityHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.east), + Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.north)); + double headingTurn = Mathf.Clamp((float)MuUtils.ClampDegrees180(desiredHeading - velocityHeading), -maxYaw, maxYaw); + double noseHeading = velocityHeading + headingTurn; + double noseRoll = (maxRoll / maxYaw) * headingTurn; + + //vertical control + percentAltitude = (vesselState.altitudeTrue / CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody)); + velocityPitch = 180 / Math.PI * Math.Atan2(vesselState.speedVertical, vesselState.speedSurface); + double nosePitch = MuUtils.Clamp(desiredFpa + stableAoA + pitchCorrection, velocityPitch + minAoA * (1 - percentAltitude), velocityPitch + maxAoA * (1 - percentAltitude)); + + core.attitude.attitudeTo(noseHeading, nosePitch, noseRoll, this); + + double flightPathAngle = 180 / Math.PI * Math.Atan2(vesselState.speedVertical, vesselState.speedSurfaceHorizontal); + double AoA = vesselState.vesselPitch - flightPathAngle; + stableAoA = (AoAtimeConstant * stableAoA + vesselState.deltaT * AoA) / (AoAtimeConstant + vesselState.deltaT); //a sort of integral error + + pitchCorrection = (pitchCorrectionTimeConstant * pitchCorrection + vesselState.deltaT * (nosePitch - vesselState.vesselPitch)) / (pitchCorrectionTimeConstant + vesselState.deltaT); + pitchCorrection = Mathf.Clamp((float)pitchCorrection, -maxPitchCorrection, maxPitchCorrection); + } } Vector3d RunwayStart() diff --git a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs index 3237e2a14..0381abbab 100644 --- a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs +++ b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs @@ -41,7 +41,7 @@ protected override void WindowGUI(int windowID) if (GUILayout.Button("Autoland")) autopilot.Autoland(this); if (autopilot.enabled && autopilot.mode == MechJebModuleSpaceplaneAutopilot.Mode.AUTOLAND - && GUILayout.Button("Abort")) autopilot.AutopilotOff(); + && GUILayout.Button("Abort")) autopilot.Abort(); GuiUtils.SimpleTextBox("Autoland glideslope:", autopilot.glideslope, "º"); @@ -56,8 +56,13 @@ protected override void WindowGUI(int windowID) GUILayout.Label("m"); GUILayout.EndHorizontal(); + GUILayout.BeginHorizontal(); + GUILayout.Label("Takeoff pitch:"); + autopilot.takeoffPitch = float.Parse(GUILayout.TextField(autopilot.takeoffPitch.ToString(), GUILayout.Width(40))); + GUILayout.EndHorizontal(); + if (autopilot.enabled && autopilot.mode == MechJebModuleSpaceplaneAutopilot.Mode.HOLD - && GUILayout.Button("Abort")) autopilot.AutopilotOff(); + && GUILayout.Button("Abort")) autopilot.Abort(); GUILayout.EndVertical(); From 54f5bf5b1051a5755f6819d80a819f5f6adcb77e Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Tue, 9 Jul 2013 17:15:16 -0700 Subject: [PATCH 33/35] Switched to universal landing coordinates Should now be able to land on different planets surfaces, given more runways are added to the list in the future. --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 26 +++++++++++--------- MechJeb2/MechJebModuleSpaceplaneGuidance.cs | 3 +++ 2 files changed, 18 insertions(+), 11 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 760a1299c..01ed3c0d3 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -56,8 +56,8 @@ public void Abort() new Runway //The runway on the island off the KSC coast. { name = "Island runway", - start = new Runway.Endpoint { latitude = -1.547474, longitude = -71.9611702, altitude = 48 }, - end = new Runway.Endpoint { latitude = -1.530174, longitude = -71.8791702, altitude = 48 } + start = new Runway.Endpoint { latitude = -1.547474, longitude = -71.9611702, altitude = 41 }, + end = new Runway.Endpoint { latitude = -1.530174, longitude = -71.8791702, altitude = 41 } } }; @@ -173,13 +173,15 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) public double runwayHeading = 0; public enum HeadingState { RIGHT, LEFT, OFF }; public HeadingState autolandHeadingState = HeadingState.OFF; + public double atmCeiling = 0; + public Vector3d runwayStart = new Vector3d(); public void DriveAutoland(FlightCtrlState s) { if (!part.vessel.Landed) { if (landed) landed = false; - Vector3d runwayStart = RunwayStart(); + runwayStart = RunwayStart(); if (!autopilotOn) AutopilotOn(); @@ -227,18 +229,20 @@ public void DriveAutoland(FlightCtrlState s) double verticalDistanceToRunway = Vector3d.Dot(vectorToRunway, vesselState.up); double horizontalDistanceToRunway = Math.Sqrt(vectorToRunway.sqrMagnitude - verticalDistanceToRunway * verticalDistanceToRunway); distanceFrom = horizontalDistanceToRunway; - if (horizontalDistanceToRunway > 10000) + atmCeiling = CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody); + if (horizontalDistanceToRunway > atmCeiling * 0.2) { - aimAltitude = 1000; - double horizontalDifference = 9900; - if (horizontalDistanceToRunway > 40000) { aimAltitude = 5000; horizontalDifference = 39900; } - if (horizontalDistanceToRunway > 80000) + aimAltitude = runway.start.altitude + atmCeiling * 0.01; + double horizontalDifference = atmCeiling * 0.199; + if (horizontalDistanceToRunway > atmCeiling * 0.5) { aimAltitude = runway.start.altitude + atmCeiling * 0.1; horizontalDifference = atmCeiling * 0.499; } + if (horizontalDistanceToRunway > atmCeiling * 4) { + //fly at a steady altitude unless below 10% altitude (7km on kerbin) aimAltitude = vesselState.altitudeTrue; - if (aimAltitude < 5000) aimAltitude = 5000; - horizontalDifference = 79900; + if (aimAltitude < atmCeiling * 0.1) aimAltitude = runway.start.altitude + atmCeiling * 0.1; + horizontalDifference = atmCeiling * 3.99; } - double setupFPA = MuUtils.Clamp(180 / Math.PI * Math.Atan2(aimAltitude - vessel.altitude, horizontalDistanceToRunway - horizontalDifference), -20, 20); + double setupFPA = MuUtils.Clamp(180 / Math.PI * Math.Atan2(aimAltitude - vessel.altitude, horizontalDistanceToRunway - horizontalDifference), -20, 10); AimVelocityVector(setupFPA, headingToWaypoint); } diff --git a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs index 0381abbab..1085f0563 100644 --- a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs +++ b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs @@ -61,6 +61,9 @@ protected override void WindowGUI(int windowID) autopilot.takeoffPitch = float.Parse(GUILayout.TextField(autopilot.takeoffPitch.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); + GUILayout.Label(autopilot.runway.start.altitude.ToString()); + GUILayout.Label(autopilot.aimAltitude.ToString()); + if (autopilot.enabled && autopilot.mode == MechJebModuleSpaceplaneAutopilot.Mode.HOLD && GUILayout.Button("Abort")) autopilot.Abort(); From 4144f73d6769df4d00f50ef9ed4ddf62261df35a Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Sat, 13 Jul 2013 01:29:59 -0700 Subject: [PATCH 34/35] Landing changes Vertical landing distance is now measured from the lowest point of the plane (hopefully being the landing gear) --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 24 +++++++++++--------- MechJeb2/MechJebModuleSpaceplaneGuidance.cs | 3 --- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 01ed3c0d3..5f2679583 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -50,14 +50,14 @@ public void Abort() new Runway //The runway at KSC { name = "KSC runway", - start = new Runway.Endpoint { latitude = -0.040633, longitude = -74.6908, altitude = 67 }, - end = new Runway.Endpoint { latitude = -0.041774, longitude = -74.5241702, altitude = 67 } + start = new Runway.Endpoint { latitude = -0.040633, longitude = -74.6908, altitude = 65 }, + end = new Runway.Endpoint { latitude = -0.041774, longitude = -74.5241702, altitude = 65 } }, new Runway //The runway on the island off the KSC coast. { name = "Island runway", - start = new Runway.Endpoint { latitude = -1.547474, longitude = -71.9611702, altitude = 41 }, - end = new Runway.Endpoint { latitude = -1.530174, longitude = -71.8791702, altitude = 41 } + start = new Runway.Endpoint { latitude = -1.547474, longitude = -71.9611702, altitude = 39 }, + end = new Runway.Endpoint { latitude = -1.530174, longitude = -71.8791702, altitude = 39 } } }; @@ -152,9 +152,12 @@ public void DriveHeadingAndAltitudeHold(FlightCtrlState s) //pitch up for takeoff if (!brakes && vesselState.time > landTime + 1.0) { - vessel.ctrlState.pitch = (float)(takeoffPitch - vesselState.vesselPitch) / 15; + vessel.ctrlState.pitch = (float)(takeoffPitch - vesselState.vesselPitch) / 10; } - vessel.ctrlState.yaw = (float)MuUtils.ClampDegrees180(targetHeading - vesselState.vesselHeading) * 0.1F; + + float targetHead = (float)targetHeading; + if (Mathf.Abs((float)(targetHead - vesselState.vesselHeading)) > 5) targetHead = (float)vesselState.vesselHeading; + vessel.ctrlState.yaw = (float)MuUtils.Clamp(MuUtils.ClampDegrees180(targetHead - vesselState.vesselHeading) * 0.01F * (25.0F / vessel.horizontalSrfSpeed), -0.1, 0.1); } double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / (30.0 * Math.Pow((CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) / (CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody) - vesselState.altitudeASL)), 5)); @@ -218,7 +221,7 @@ public void DriveAutoland(FlightCtrlState s) autolandHeadingState = HeadingState.OFF; //stop any rolling and aim down runway before touching down - if ((vesselState.CoM - runwayStart).magnitude < 500.0) + if ((vesselState.CoM - runwayStart).magnitude < 200.0) { Vector3d runwayDir = runway.End(vesselState.CoM) - runway.Start(vesselState.CoM); runwayHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(runwayDir, vesselState.east), Vector3d.Dot(runwayDir, vesselState.north)); @@ -226,7 +229,7 @@ public void DriveAutoland(FlightCtrlState s) } Vector3d vectorToRunway = runwayStart - vesselState.CoM; - double verticalDistanceToRunway = Vector3d.Dot(vectorToRunway, vesselState.up); + double verticalDistanceToRunway = Vector3d.Dot(vectorToRunway, vesselState.up) - (vesselState.altitudeTrue - vesselState.altitudeBottom); double horizontalDistanceToRunway = Math.Sqrt(vectorToRunway.sqrMagnitude - verticalDistanceToRunway * verticalDistanceToRunway); distanceFrom = horizontalDistanceToRunway; atmCeiling = CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody); @@ -276,9 +279,8 @@ public void DriveAutoland(FlightCtrlState s) if (Vector3d.Dot(runwayDir, vesselState.forward) < 0) runwayDir *= -1; runwayHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(runwayDir, vesselState.east), Vector3d.Dot(runwayDir, vesselState.north)); vessel.ctrlState.pitch = (float)MuUtils.Clamp(-vesselState.vesselPitch * 0.1, -0.5, 0); - vessel.ctrlState.yaw = (float)MuUtils.Clamp(MuUtils.ClampDegrees180(runwayHeading - vesselState.vesselHeading) * 0.1F, -0.1, 0.1); - if (vesselState.vesselRoll < -1) vessel.ctrlState.roll = 1; - if (vesselState.vesselRoll > 1) vessel.ctrlState.roll = -1; + vessel.ctrlState.yaw = (float)MuUtils.Clamp(MuUtils.ClampDegrees180(runwayHeading - vesselState.vesselHeading) * 0.01F * (25.0F / vessel.horizontalSrfSpeed), -0.1, 0.1); + vessel.ctrlState.roll = (float)MuUtils.Clamp(MuUtils.ClampDegrees180(-vesselState.vesselRoll) * 0.01F * (25.0F / vessel.horizontalSrfSpeed), -0.1, 0.1); } } diff --git a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs index 1085f0563..0381abbab 100644 --- a/MechJeb2/MechJebModuleSpaceplaneGuidance.cs +++ b/MechJeb2/MechJebModuleSpaceplaneGuidance.cs @@ -61,9 +61,6 @@ protected override void WindowGUI(int windowID) autopilot.takeoffPitch = float.Parse(GUILayout.TextField(autopilot.takeoffPitch.ToString(), GUILayout.Width(40))); GUILayout.EndHorizontal(); - GUILayout.Label(autopilot.runway.start.altitude.ToString()); - GUILayout.Label(autopilot.aimAltitude.ToString()); - if (autopilot.enabled && autopilot.mode == MechJebModuleSpaceplaneAutopilot.Mode.HOLD && GUILayout.Button("Abort")) autopilot.Abort(); From 124e29ec009b078db81d0ca8d2ca8e811dfda28f Mon Sep 17 00:00:00 2001 From: Christian Moe Date: Sat, 13 Jul 2013 17:00:04 -0700 Subject: [PATCH 35/35] Landing roll fix --- MechJeb2/MechJebModuleSpaceplaneAutopilot.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs index 5f2679583..256fabb31 100644 --- a/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs +++ b/MechJeb2/MechJebModuleSpaceplaneAutopilot.cs @@ -280,7 +280,7 @@ public void DriveAutoland(FlightCtrlState s) runwayHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(runwayDir, vesselState.east), Vector3d.Dot(runwayDir, vesselState.north)); vessel.ctrlState.pitch = (float)MuUtils.Clamp(-vesselState.vesselPitch * 0.1, -0.5, 0); vessel.ctrlState.yaw = (float)MuUtils.Clamp(MuUtils.ClampDegrees180(runwayHeading - vesselState.vesselHeading) * 0.01F * (25.0F / vessel.horizontalSrfSpeed), -0.1, 0.1); - vessel.ctrlState.roll = (float)MuUtils.Clamp(MuUtils.ClampDegrees180(-vesselState.vesselRoll) * 0.01F * (25.0F / vessel.horizontalSrfSpeed), -0.1, 0.1); + vessel.ctrlState.roll = (float)MuUtils.Clamp(MuUtils.ClampDegrees180(vesselState.vesselRoll) * 0.01F * (25.0F / vessel.horizontalSrfSpeed), -0.1, 0.1); } }