Raf04 sarbian

MechJeb2/MechJebModuleSpaceplaneAutopilot.cs

@@ -10,39 +10,54 @@ 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 = 
         {
             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 = 48 },
-                end = new Runway.Endpoint { latitude = -1.530174, longitude = -71.8791702, altitude = 48 }
+                start = new Runway.Endpoint { latitude = -1.547474, longitude = -71.9611702, altitude = 39 },
+                end = new Runway.Endpoint { latitude = -1.530174, longitude = -71.8791702, altitude = 39 }
             }
         };
 
@@ -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,101 @@ 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) / 10;
+                }
+
+                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));
+            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 double atmCeiling = 0;
+        public Vector3d runwayStart = new Vector3d();
         public void DriveAutoland(FlightCtrlState s)
         {
             if (!part.vessel.Landed)
             {
-                Vector3d runwayStart = RunwayStart();
+                if (landed) landed = false;
+
+                runwayStart = RunwayStart();
+
+                if (!autopilotOn)
+                    AutopilotOn();
 
+                //prepare for landing
                 if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 1000.0)
                 {
                     vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, true);
@@ -100,53 +198,129 @@ 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 < 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));
+                    headingToWaypoint = runwayHeading;
+                }
 
                 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);
-                double flightPathAngleToRunway = 180 / Math.PI * Math.Atan2(verticalDistanceToRunway, horizontalDistanceToRunway);
-                double desiredFPA = Mathf.Clamp((float)(flightPathAngleToRunway + 3 * (flightPathAngleToRunway + glideslope)), -20.0F, 0.0F);
+                distanceFrom = horizontalDistanceToRunway;
+                atmCeiling = CelestialBodyExtensions.RealMaxAtmosphereAltitude(mainBody);
+                if (horizontalDistanceToRunway > atmCeiling * 0.2)
+                {
+                    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 < 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, 10);
+
+                    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.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);
             }
         }
 
         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()

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();