Example #1
0
        public static string ToStringDecimal(double latitude, double longitude, bool newline = false, int precision = 3)
        {
            double clampedLongitude = MuUtils.ClampDegrees180(longitude);
            double latitudeAbs      = Math.Abs(latitude);
            double longitudeAbs     = Math.Abs(clampedLongitude);

            return(latitudeAbs.ToString("F" + precision) + "° " + (latitude > 0 ? "N" : "S") + (newline ? "\n" : ", ")
                   + longitudeAbs.ToString("F" + precision) + "° " + (clampedLongitude > 0 ? "E" : "W"));
        }
Example #2
0
        private void UpdatePredictionPI()
        {
            // velcity relative to the target is the minus of the velocity relative to the vessel
            // (The PID moves the vessel to zero in the target frame)
            Omega = -ac.vessel.angularVelocity;

            UpdateError();

            // see https://archive.is/NqoUm and the "Alt Hold Controller", the acceleration PID is not implemented so we only
            // have the first two PIDs in the cascade.
            for (int i = 0; i < 3; i++)
            {
                MaxAlpha[i] = ControlTorque[i] / ac.vesselState.MoI[i];
                // scale the LD the user inputs to get the actual LD we use
                // (this was determined entirely empirically by seeing that vessels with appx 1000x range of MaxAlpha
                // needed a roughly 10x different LD, so by scaling here, we produce a user tunable which should be
                // fairly constant across a large range of vessels)
                double effLD = LD * Math.Pow(MaxAlpha[i], 1.0 / 3.0);
                double Gain  = Math.Sqrt(0.5 * MaxAlpha[i] / effLD);
                if (Math.Abs(errorVector[i]) <= 2 * effLD)
                {
                    // linear ramp down of acceleration
                    TargetOmega[i] = Gain * errorVector[i];
                }
                else
                {
                    // v = - sqrt(2 * F * x / m) is target stopping velocity based on distance
                    TargetOmega[i] = Math.Sqrt(2 * MaxAlpha[i] * (Math.Abs(errorVector[i]) - effLD)) * Math.Sign(errorVector[i]);
                }

                if (useStoppingTime)
                {
                    MaxOmega[i] = MaxAlpha[i] * maxStoppingTime;
                    if (useFlipTime)
                    {
                        MaxOmega[i] = Math.Max(MaxOmega[i], Math.PI / minFlipTime);
                    }
                    TargetOmega[i] = MuUtils.Clamp(TargetOmega[i], -MaxOmega[i], MaxOmega[i]);
                }
            }

            if (useControlRange && errorTotal * Mathf.Rad2Deg > rollControlRange)
            {
                TargetOmega[1] = 0;
                Pid[1].ResetI();
            }

            for (int i = 0; i < 3; i++)
            {
                Pid[i].Ki = Ki;
                Pid[i].Kp = Kp / TimeWarp.CurrentRate;
                Pid[i].Kd = Kd;

                Actuation[i]    = Pid[i].Update(Omega[i] / MaxAlpha[i], TargetOmega[i] / MaxAlpha[i], 1.0);
                TargetTorque[i] = Actuation[i] * ControlTorque[i]; // for display
            }
        }
Example #3
0
            public override AutopilotStep Drive(FlightCtrlState s)
            {
                // primary directions come from trajectories, but we may change them in descend phase to stay on course
                targetInfo.update();
                switch (phase)
                {
                case Phase.waitForEntry:
                    status = "Holding entry attitude, no corrections";
                    //just hold attitude in this phase
                    core.attitude.attitudeTo(TrajectoriesConnector.API.PlannedOrientation().Value, AttitudeReference.SURFACE_VELOCITY, this);
                    if (vesselState.altitudeASL < mainBody.atmosphereDepth)
                    {
                        phase = Phase.descend;
                    }
                    break;

                case Phase.descend:
                    descend();
                    if (core.landing.deployChutes &&
                        vesselState.parachutes.Any(p => p.deploymentSafeState == ModuleParachute.deploymentSafeStates.SAFE && p.deploymentState == ModuleParachute.deploymentStates.STOWED))
                    {
                        phase = Phase.parachutes;
                        core.attitude.attitudeTo(TrajectoriesConnector.API.PlannedOrientation().Value, AttitudeReference.SURFACE_VELOCITY, this);
                        core.thrust.targetThrottle = 0;
                    }
                    break;

                case Phase.parachutes:
                    parachuteInfo.update(vesselState.speedSurface, vesselState.atmosphericDensity);
                    double dist = Vector3d.Dot(targetInfo.distanceTarget, vesselState.horizontalSurface) - parachuteInfo.breakDistance;
                    Debug.Log(String.Format("Waiting for parachutes to open Speed:{0:F0} dist:{1:F0} break:{2:F0} maxSpeed:{3:F0} ", vesselState.speedSurface, targetInfo.forwardDistance, dist, parachuteInfo.maxSpeed));
                    status = "Waiting " + MuUtils.ToSI(dist) + "m with parachutes deploy to hit target";
                    // control parachute opening
                    if (dist <= 0)
                    {
                        deployParachutes();
                    }
                    if (vesselState.parachutes.All(p => p.deploymentState != ModuleParachute.deploymentStates.STOWED))
                    {
                        return(new FinalDescent(core));
                    }
                    break;
                }


                if (vesselState.altitudeTrue < 1000 || vesselState.speedSurface < 200)
                {
                    if (core.landing.deployChutes)
                    {
                        deployParachutes();
                    }
                    return(new FinalDescent(core));
                }

                return(this);
            }
Example #4
0
        public EditableAngle(double angle)
        {
            angle = MuUtils.ClampDegrees180(angle);

            negative = (angle < 0);
            angle    = Math.Abs(angle);
            degrees  = (int)angle;
            angle   -= degrees;
            minutes  = (int)(60 * angle);
            angle   -= minutes / 60;
            seconds  = Math.Round(3600 * angle);
        }
Example #5
0
        public override ManeuverParameters MakeNodeImpl(Orbit o, double universalTime, MechJebModuleTargetController target)
        {
            double UT = timeSelector.ComputeManeuverTime(o, universalTime, target);

            if (o.referenceBody.Radius + newApA < o.Radius(UT))
            {
                string burnAltitude = MuUtils.ToSI(o.Radius(UT) - o.referenceBody.Radius) + "m";
                throw new OperationException("new apoapsis cannot be lower than the altitude of the burn (" + burnAltitude + ")");
            }

            return(new ManeuverParameters(OrbitalManeuverCalculator.DeltaVToChangeApoapsis(o, UT, newApA + o.referenceBody.Radius), UT));
        }
Example #6
0
            public override AutopilotStep OnFixedUpdate()
            {
                if (!vessel.patchedConicsUnlocked() || vessel.patchedConicSolver.maneuverNodes.Count == 0)
                {
                    return(doAfterExecution);
                }

                node = vessel.patchedConicSolver.maneuverNodes[0];
                double dVLeft = node.GetBurnVector(orbit).magnitude;

                if (dVLeft < core.node.tolerance && core.attitude.attitudeAngleFromTarget() > 5)
                {
                    burnTriggered = false;

                    node.RemoveSelf();

                    return(this); // we are done for this frame, continue in next
                }

                double halfBurnTime;
                double burnTime = core.node.BurnTime(dVLeft, out halfBurnTime);

                double timeToNode = node.UT - vesselState.time;

                status = "Moving to node";

                if ((!double.IsInfinity(halfBurnTime) && halfBurnTime > 0 && timeToNode < halfBurnTime) || timeToNode < 0)
                {
                    burnTriggered = true;
                    status        = "Executing node";
                    if (!MuUtils.PhysicsRunning())
                    {
                        core.warp.MinimumWarp();
                    }
                }

                //autowarp, but only if we're already aligned with the node
                if (core.node.autowarp && !burnTriggered)
                {
                    if ((core.attitude.attitudeAngleFromTarget() < 1 && core.vessel.angularVelocity.magnitude < 0.01) || (core.attitude.attitudeAngleFromTarget() < 10 && !MuUtils.PhysicsRunning()))
                    {
                        core.warp.WarpToUT(node.UT - halfBurnTime - core.node.leadTime);
                    }
                    else if (!MuUtils.PhysicsRunning() && core.attitude.attitudeAngleFromTarget() > 10 && timeToNode < 600)
                    {
                        //realign
                        core.warp.MinimumWarp();
                    }
                }

                return(this);
            }
Example #7
0
        public override void GUI()
        {
            useInertia = GUILayout.Toggle(useInertia, "useInertia");

            GUILayout.BeginHorizontal();
            GUILayout.Label("MaxStoppingTime", GUILayout.ExpandWidth(false));
            maxStoppingTime.text = GUILayout.TextField(maxStoppingTime.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            useControlRange       = GUILayout.Toggle(useControlRange, "RollControlRange", GUILayout.ExpandWidth(false));
            rollControlRange.text = GUILayout.TextField(rollControlRange.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
            GUILayout.EndHorizontal();

            // Not used yet
            //GuiUtils.SimpleTextBox("Maximum Relative Angular Velocity", kWlimit, "%");
            //double tmp_kWlimit = kWlimit;
            //tmp_kWlimit = (EditableDouble)GUILayout.HorizontalSlider((float)tmp_kWlimit, 0.0F, 1.0F);
            //
            //const int sliderPrecision = 3;
            //if (Math.Round(Math.Abs(tmp_kWlimit - kWlimit), sliderPrecision) > 0)
            //{
            //    kWlimit.val = Math.Round(tmp_kWlimit, sliderPrecision);
            //}

            GUILayout.BeginHorizontal();
            GUILayout.Label("Actuation", GUILayout.ExpandWidth(true));
            GUILayout.Label(MuUtils.PrettyPrint(Actuation), GUILayout.ExpandWidth(false));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("phiVector", GUILayout.ExpandWidth(true));
            GUILayout.Label(MuUtils.PrettyPrint(phiVector), GUILayout.ExpandWidth(false));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("TargetTorque", GUILayout.ExpandWidth(true));
            GUILayout.Label(MuUtils.PrettyPrint(TargetTorque), GUILayout.ExpandWidth(false));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("ControlTorque", GUILayout.ExpandWidth(true));
            GUILayout.Label(MuUtils.PrettyPrint(ControlTorque), GUILayout.ExpandWidth(false));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("Inertia", GUILayout.ExpandWidth(true));
            GUILayout.Label("|" + ac.inertia.magnitude.ToString("F3") + "| " + MuUtils.PrettyPrint(ac.inertia), GUILayout.ExpandWidth(false));
            GUILayout.EndHorizontal();
        }
        public override ManeuverParameters MakeNodeImpl(Orbit o, double universalTime, MechJebModuleTargetController target)
        {
            double UT = timeSelector.ComputeManeuverTime(o, universalTime, target);

            if (o.referenceBody.Radius + newPeA > o.Radius(UT))
            {
                string burnAltitude = MuUtils.ToSI(o.Radius(UT) - o.referenceBody.Radius) + "m";
                throw new OperationException("new periapsis cannot be higher than the altitude of the burn (" + burnAltitude + ")");
            }
            else if (newPeA < -o.referenceBody.Radius)
            {
                throw new OperationException("new periapsis cannot be lower than minus the radius of " + o.referenceBody.displayName + "(-" + MuUtils.ToSI(o.referenceBody.Radius, 3) + "m)");
            }

            return(new ManeuverParameters(OrbitalManeuverCalculator.DeltaVToChangePeriapsis(o, UT, newPeA + o.referenceBody.Radius), UT));
        }
Example #9
0
        public static Coordinates GetMouseCoordinates(CelestialBody body)
        {
            Ray mouseRay = PlanetariumCamera.Camera.ScreenPointToRay(Input.mousePosition);

            mouseRay.origin = ScaledSpace.ScaledToLocalSpace(mouseRay.origin);
            Vector3d relOrigin = mouseRay.origin - body.position;
            Vector3d relSurfacePosition;
            double   curRadius  = body.pqsController.radiusMax;
            double   lastRadius = 0;
            double   error      = 0;
            int      loops      = 0;
            float    st         = Time.time;

            while (loops < 50)
            {
                if (PQS.LineSphereIntersection(relOrigin, mouseRay.direction, curRadius, out relSurfacePosition))
                {
                    Vector3d surfacePoint = body.position + relSurfacePosition;
                    double   alt          = body.pqsController.GetSurfaceHeight(QuaternionD.AngleAxis(body.GetLongitude(surfacePoint), Vector3d.down) * QuaternionD.AngleAxis(body.GetLatitude(surfacePoint), Vector3d.forward) * Vector3d.right);
                    error = Math.Abs(curRadius - alt);
                    if (error < (body.pqsController.radiusMax - body.pqsController.radiusMin) / 100)
                    {
                        return(new Coordinates(body.GetLatitude(surfacePoint), MuUtils.ClampDegrees180(body.GetLongitude(surfacePoint))));
                    }
                    else
                    {
                        lastRadius = curRadius;
                        curRadius  = alt;
                        loops++;
                    }
                }
                else
                {
                    if (loops == 0)
                    {
                        break;
                    }
                    else
                    { // Went too low, needs to try higher
                        curRadius = (lastRadius * 9 + curRadius) / 10;
                        loops++;
                    }
                }
            }

            return(null);
        }
Example #10
0
    public static Coordinates GetMouseCoordinates(CelestialBody body)
    {
        Ray mouseRay = PlanetariumCamera.Camera.ScreenPointToRay(Input.mousePosition);

        mouseRay.origin = ScaledSpace.ScaledToLocalSpace(mouseRay.origin);
        Vector3d relOrigin = mouseRay.origin - body.position;
        Vector3d relSurfacePosition;

        if (PQS.LineSphereIntersection(relOrigin, mouseRay.direction, body.Radius, out relSurfacePosition))
        {
            Vector3d surfacePoint = body.position + relSurfacePosition;
            return(new Coordinates(body.GetLatitude(surfacePoint), MuUtils.ClampDegrees180(body.GetLongitude(surfacePoint))));
        }
        else
        {
            return(null);
        }
    }
Example #11
0
            public override AutopilotStep OnFixedUpdate()
            {
                Vector3d targetRadialVector  = mainBody.GetRelSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0);
                Vector3d currentRadialVector = vesselState.CoM - mainBody.position;
                double   angleToTarget       = Vector3d.Angle(targetRadialVector, currentRadialVector);
                bool     approaching         = Vector3d.Dot(targetRadialVector - currentRadialVector, vesselState.orbitalVelocity) > 0;

                if (!planeChangeTriggered && approaching && (angleToTarget > 80) && (angleToTarget < 90))
                {
                    if (!MuUtils.PhysicsRunning())
                    {
                        core.warp.MinimumWarp(true);
                    }
                    planeChangeTriggered = true;
                }

                if (planeChangeTriggered)
                {
                    Vector3d horizontalToTarget = ComputePlaneChange();
                    Vector3d finalVelocity      = Quaternion.FromToRotation(vesselState.horizontalOrbit, horizontalToTarget) * vesselState.orbitalVelocity;

                    Vector3d deltaV = finalVelocity - vesselState.orbitalVelocity;
                    //burn normal+ or normal- to avoid dropping the Pe:
                    Vector3d burnDir = Vector3d.Exclude(vesselState.up, Vector3d.Exclude(vesselState.orbitalVelocity, deltaV));
                    planeChangeDVLeft = UtilMath.Deg2Rad * Vector3d.Angle(finalVelocity, vesselState.orbitalVelocity) * vesselState.speedOrbitHorizontal;
                    core.attitude.attitudeTo(burnDir, AttitudeReference.INERTIAL, core.landing);
                    status = "Executing low orbit plane change of about " + planeChangeDVLeft.ToString("F0") + " m/s";

                    if (planeChangeDVLeft < 0.1F)
                    {
                        return(new LowDeorbitBurn(core));
                    }
                }
                else
                {
                    if (core.node.autowarp)
                    {
                        core.warp.WarpRegularAtRate((float)(orbit.period / 6));
                    }
                    status = "Moving to low orbit plane change burn point";
                }

                return(this);
            }
Example #12
0
        public void UpdatePhi()
        {
            Transform vesselTransform = ac.vessel.ReferenceTransform;

            // 1. The Euler(-90) here is because the unity transform puts "up" as the pointy end, which is wrong.  The rotation means that
            // "forward" becomes the pointy end, and "up" and "right" correctly define e.g. AoA/pitch and AoS/yaw.  This is just KSP being KSP.
            // 2. We then use the inverse ship rotation to transform the requested attitude into the ship frame.
            Quaternion deltaRotation = Quaternion.Inverse(vesselTransform.transform.rotation * Quaternion.Euler(-90, 0, 0)) * ac.RequestedAttitude;

            // get us some euler angles for the target transform
            Vector3d ea    = deltaRotation.eulerAngles;
            double   pitch = ea[0] * UtilMath.Deg2Rad;
            double   yaw   = ea[1] * UtilMath.Deg2Rad;
            double   roll  = ea[2] * UtilMath.Deg2Rad;

            // law of cosines for the "distance" of the miss in radians
            phiTotal = Math.Acos(MuUtils.Clamp(Math.Cos(pitch) * Math.Cos(yaw), -1, 1));

            // this is the initial direction of the great circle route of the requested transform
            // (pitch is latitude, yaw is -longitude, and we are "navigating" from 0,0)
            Vector3d temp = new Vector3d(Math.Sin(pitch), Math.Cos(pitch) * Math.Sin(-yaw), 0);

            temp = temp.normalized * phiTotal;

            // we assemble phi in the pitch, roll, yaw basis that vessel.MOI uses (right handed basis)
            Vector3d phi = new Vector3d(
                MuUtils.ClampRadiansPi(temp[0]),    // pitch distance around the geodesic
                MuUtils.ClampRadiansPi(roll),
                MuUtils.ClampRadiansPi(temp[1])     // yaw distance around the geodesic
                );

            phi.Scale(ac.AxisState);

            if (useInertia)
            {
                phi -= ac.inertia;
            }

            phiVector = phi;
        }
Example #13
0
        public override void GUI()
        {
            if (GUILayout.Button(Localizer.Format("#MechJeb_adv_reset_button")))//"Reset"
            {
                ResetConfig();
            }

            Tf_autoTune = GUILayout.Toggle(Tf_autoTune, Localizer.Format("#MechJeb_AttitudeController_checkbox1"));//" Auto-tuning"

            GUILayout.BeginHorizontal();
            GUILayout.Space(20);
            GUILayout.BeginVertical();

            if (!Tf_autoTune)
            {
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label1"));//"Larger ship do better with a larger Tf"

                GUILayout.BeginHorizontal();
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label2"), GUILayout.ExpandWidth(true));  //"Tf (s)"
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label3"), GUILayout.ExpandWidth(false)); //"P"
                UI_TfX.text = GUILayout.TextField(UI_TfX.text, GUILayout.ExpandWidth(true), GUILayout.Width(40));
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label4"), GUILayout.ExpandWidth(false)); //"Y"
                UI_TfY.text = GUILayout.TextField(UI_TfY.text, GUILayout.ExpandWidth(true), GUILayout.Width(40));
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label5"), GUILayout.ExpandWidth(false)); //"R"
                UI_TfZ.text = GUILayout.TextField(UI_TfZ.text, GUILayout.ExpandWidth(true), GUILayout.Width(40));
                GUILayout.EndHorizontal();

                UI_TfX = Math.Max(0.01, UI_TfX);
                UI_TfY = Math.Max(0.01, UI_TfY);
                UI_TfZ = Math.Max(0.01, UI_TfZ);
            }
            else
            {
                GUILayout.BeginHorizontal();
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label6"), GUILayout.ExpandWidth(true));//"Tf"
                GUILayout.Label(MuUtils.PrettyPrint(TfV), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label7"), GUILayout.ExpandWidth(true)); //"Tf range"
                GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_AttitudeController_label8"), UI_TfMin, "", 50);     //"min"
                UI_TfMin = Math.Max(UI_TfMin, 0.01);
                GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_AttitudeController_label9"), UI_TfMax, "", 50);     //"max"
                UI_TfMax = Math.Max(UI_TfMax, 0.01);
                GUILayout.EndHorizontal();
            }
            GUILayout.EndVertical();
            GUILayout.EndHorizontal();

            bool newLowPassFilter = GUILayout.Toggle(lowPassFilter, Localizer.Format("#MechJeb_AttitudeController_checkbox2"));//" Low Pass Filter"

            if (lowPassFilter != newLowPassFilter)
            {
                setPIDParameters();
                lowPassFilter = newLowPassFilter;
            }

            GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_PIDF")); //"PID factors"
            GuiUtils.SimpleTextBox("Kd = ", UI_kdFactor, " / Tf", 50);             //
            UI_kdFactor = Math.Max(UI_kdFactor, 0.01);
            GuiUtils.SimpleTextBox("Kp = pid.Kd / (", UI_kpFactor, " * Math.Sqrt(2) * Tf)", 50);
            UI_kpFactor = Math.Max(UI_kpFactor, 0.01);
            GuiUtils.SimpleTextBox("Ki = pid.Kp / (", UI_kiFactor, " * Math.Sqrt(2) * Tf)", 50);
            UI_kiFactor = Math.Max(UI_kiFactor, 0.01);
            GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_AttitudeController_PIDFactor1"), UI_deadband, "", 50);//"Deadband = "
            deadband = Math.Max(UI_deadband, 0.0);

            GuiUtils.SimpleTextBox(Localizer.Format("#MechJeb_AttitudeController_label11"), kWlimit, "%");//"Maximum Relative Angular Velocity"
            double tmp_kWlimit = kWlimit;

            tmp_kWlimit = (EditableDouble)GUILayout.HorizontalSlider((float)tmp_kWlimit, 0.0F, 1.0F);

            const int sliderPrecision = 3;

            if (Math.Round(Math.Abs(tmp_kWlimit - kWlimit), sliderPrecision) > 0)
            {
                kWlimit = Math.Round(tmp_kWlimit, sliderPrecision);
            }

            //showInfos = GUILayout.Toggle(showInfos, "Show Numbers");
            //if (showInfos)
            {
                GUILayout.BeginHorizontal();
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label12"), GUILayout.ExpandWidth(true));//"Kp"
                GUILayout.Label(MuUtils.PrettyPrint(pid.Kp), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label13"), GUILayout.ExpandWidth(true));//"Ki"
                GUILayout.Label(MuUtils.PrettyPrint(pid.Ki), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label14"), GUILayout.ExpandWidth(true));//"Kd"
                GUILayout.Label(MuUtils.PrettyPrint(pid.Kd), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label15"), GUILayout.ExpandWidth(true));//"Error"
                GUILayout.Label(MuUtils.PrettyPrint(error * Mathf.Rad2Deg), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label16"), GUILayout.ExpandWidth(true));//"prop. action."
                GUILayout.Label(MuUtils.PrettyPrint(pid.propAct), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label17"), GUILayout.ExpandWidth(true));//"deriv. action"
                GUILayout.Label(MuUtils.PrettyPrint(pid.derivativeAct), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label18"), GUILayout.ExpandWidth(true));//"integral action."
                GUILayout.Label(MuUtils.PrettyPrint(pid.intAccum), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label19"), GUILayout.ExpandWidth(true));//"PID Action"
                GUILayout.Label(MuUtils.PrettyPrint(pidAction), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label(Localizer.Format("#MechJeb_AttitudeController_label20"), GUILayout.ExpandWidth(true));//"Inertia"
                GUILayout.Label("|" + ac.inertia.magnitude.ToString("F3") + "| " + MuUtils.PrettyPrint(ac.inertia),
                                GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();
            }

            if (!Tf_autoTune)
            {
                if (TfV.x != UI_TfX || TfV.y != UI_TfY || TfV.z != UI_TfZ)
                {
                    TfV.x = UI_TfX;
                    TfV.y = UI_TfY;
                    TfV.z = UI_TfZ;
                    setPIDParameters();
                }
            }
            else
            {
                if (TfMin != UI_TfMin || TfMax != UI_TfMax)
                {
                    TfMin = UI_TfMin;
                    TfMax = UI_TfMax;
                    setPIDParameters();
                }
            }
            if (kpFactor != UI_kpFactor || kiFactor != UI_kiFactor || kdFactor != UI_kdFactor)
            {
                kpFactor = UI_kpFactor;
                kiFactor = UI_kiFactor;
                kdFactor = UI_kdFactor;
                setPIDParameters();
            }
        }
Example #14
0
        public override ManeuverParameters MakeNodeImpl(Orbit o, double universalTime, MechJebModuleTargetController target)
        {
            double UT = timeSelector.ComputeManeuverTime(o, universalTime, target);

            if (2 * newSMA > o.Radius(UT) + o.referenceBody.sphereOfInfluence)
            {
                errorMessage = "Warning: new Semi-Major Axis is very large, and may result in a hyberbolic orbit";
            }

            if (o.Radius(UT) > 2 * newSMA)
            {
                throw new OperationException("cannot make Semi-Major Axis less than twice the burn altitude plus the radius of " + o.referenceBody.theName + "(" + MuUtils.ToSI(o.referenceBody.Radius, 3) + "m)");
            }

            return(new ManeuverParameters(OrbitalManeuverCalculator.DeltaVForSemiMajorAxis(o, UT, newSMA), UT));
        }
Example #15
0
            public override AutopilotStep OnFixedUpdate()
            {
                //Decide when we will start the deorbit burn:
                double stoppingDistance  = Math.Pow(vesselState.speedSurfaceHorizontal, 2) / (2 * vesselState.limitedMaxThrustAccel);
                double triggerDistance   = lowDeorbitBurnTriggerFactor * stoppingDistance;
                double heightAboveTarget = vesselState.altitudeASL - core.landing.DecelerationEndAltitude();

                if (triggerDistance < heightAboveTarget)
                {
                    triggerDistance = heightAboveTarget;
                }

                //See if it's time to start the deorbit burn:
                double rangeToTarget = Vector3d.Exclude(vesselState.up, core.target.GetPositionTargetPosition() - vesselState.CoM).magnitude;

                if (!deorbitBurnTriggered && rangeToTarget < triggerDistance)
                {
                    if (!MuUtils.PhysicsRunning())
                    {
                        core.warp.MinimumWarp(true);
                    }
                    deorbitBurnTriggered = true;
                }

                if (deorbitBurnTriggered)
                {
                    status = Localizer.Format("#MechJeb_LandingGuidance_Status11");                      //"Executing low deorbit burn"
                }
                else
                {
                    status = Localizer.Format("#MechJeb_LandingGuidance_Status12"); //"Moving to low deorbit burn point"
                }
                //Warp toward deorbit burn if it hasn't been triggerd yet:
                if (!deorbitBurnTriggered && core.node.autowarp && rangeToTarget > 2 * triggerDistance)
                {
                    core.warp.WarpRegularAtRate((float)(orbit.period / 6));
                }
                if (rangeToTarget < triggerDistance && !MuUtils.PhysicsRunning())
                {
                    core.warp.MinimumWarp();
                }

                //By default, thrust straight back at max throttle
                Vector3d thrustDirection = -vesselState.surfaceVelocity.normalized;

                lowDeorbitBurnMaxThrottle = 1;

                //If we are burning, we watch the predicted landing site and switch to the braking
                //burn when the predicted landing site crosses the target. We also use the predictions
                //to steer the predicted landing site toward the target
                if (deorbitBurnTriggered && core.landing.PredictionReady)
                {
                    //angle slightly left or right to fix any cross-range error in the predicted landing site:
                    Vector3d     horizontalToLandingSite = Vector3d.Exclude(vesselState.up, core.landing.LandingSite - vesselState.CoM).normalized;
                    Vector3d     horizontalToTarget      = Vector3d.Exclude(vesselState.up, core.target.GetPositionTargetPosition() - vesselState.CoM).normalized;
                    const double angleGain       = 4;
                    Vector3d     angleCorrection = angleGain * (horizontalToTarget - horizontalToLandingSite);
                    if (angleCorrection.magnitude > 0.1)
                    {
                        angleCorrection *= 0.1 / angleCorrection.magnitude;
                    }
                    thrustDirection = (thrustDirection + angleCorrection).normalized;

                    double rangeToLandingSite = Vector3d.Exclude(vesselState.up, core.landing.LandingSite - vesselState.CoM).magnitude;
                    double maxAllowedSpeed    = core.landing.MaxAllowedSpeed();

                    if (!lowDeorbitEndConditionSet && Vector3d.Distance(core.landing.LandingSite, vesselState.CoM) < mainBody.Radius + vesselState.altitudeASL)
                    {
                        lowDeorbitEndOnLandingSiteNearer = rangeToLandingSite > rangeToTarget;
                        lowDeorbitEndConditionSet        = true;
                    }

                    lowDeorbitBurnMaxThrottle = 1;

                    if (orbit.PeA < 0)
                    {
                        if (rangeToLandingSite > rangeToTarget)
                        {
                            if (lowDeorbitEndConditionSet && !lowDeorbitEndOnLandingSiteNearer)
                            {
                                core.thrust.targetThrottle = 0;
                                return(new DecelerationBurn(core));
                            }

                            double maxAllowedSpeedAfterDt = core.landing.MaxAllowedSpeedAfterDt(vesselState.deltaT);
                            double speedAfterDt           = vesselState.speedSurface + vesselState.deltaT * Vector3d.Dot(vesselState.gravityForce, vesselState.surfaceVelocity.normalized);
                            double throttleToMaintainLandingSite;
                            if (vesselState.speedSurface < maxAllowedSpeed)
                            {
                                throttleToMaintainLandingSite = 0;
                            }
                            else
                            {
                                throttleToMaintainLandingSite = (speedAfterDt - maxAllowedSpeedAfterDt) / (vesselState.deltaT * vesselState.maxThrustAccel);
                            }

                            lowDeorbitBurnMaxThrottle = throttleToMaintainLandingSite + 1 * (rangeToLandingSite / rangeToTarget - 1) + 0.2;
                        }
                        else
                        {
                            if (lowDeorbitEndConditionSet && lowDeorbitEndOnLandingSiteNearer)
                            {
                                core.thrust.targetThrottle = 0;
                                return(new DecelerationBurn(core));
                            }
                            else
                            {
                                lowDeorbitBurnMaxThrottle = 0;
                                status = Localizer.Format("#MechJeb_LandingGuidance_Status13");//"Deorbit burn complete: waiting for the right moment to start braking"
                            }
                        }
                    }
                }

                core.attitude.attitudeTo(thrustDirection, AttitudeReference.INERTIAL, core.landing);

                return(this);
            }
Example #16
0
            public override AutopilotStep OnFixedUpdate()
            {
                if (vesselState.altitudeASL < core.landing.DecelerationEndAltitude() + 5)
                {
                    core.warp.MinimumWarp();

                    if (core.landing.UseAtmosphereToBrake())
                    {
                        return(new FinalDescent(core));
                    }
                    else
                    {
                        return(new KillHorizontalVelocity(core));
                    }
                }

                double decelerationStartTime = (core.landing.prediction.trajectory.Any() ? core.landing.prediction.trajectory.First().UT : vesselState.time);

                if (!(core.landing.minThrust > 0 && core.thrust.targetThrottle > 0) && decelerationStartTime - vesselState.time > 5)
                {
                    core.thrust.targetThrottle = 0;

                    status = "Warping to start of braking burn.";

                    //warp to deceleration start
                    Vector3d decelerationStartAttitude = -orbit.SwappedOrbitalVelocityAtUT(decelerationStartTime);
                    decelerationStartAttitude += mainBody.getRFrmVel(orbit.SwappedAbsolutePositionAtUT(decelerationStartTime));
                    decelerationStartAttitude  = decelerationStartAttitude.normalized;
                    core.attitude.attitudeTo(decelerationStartAttitude, AttitudeReference.INERTIAL, core.landing);
                    bool warpReady = core.attitude.attitudeAngleFromTarget() < 5;

                    if (warpReady && core.node.autowarp)
                    {
                        core.warp.WarpToUT(decelerationStartTime - 5);
                    }
                    else if (!MuUtils.PhysicsRunning())
                    {
                        core.warp.MinimumWarp();
                    }
                    return(this);
                }

                Vector3d desiredThrustVector = -vesselState.surfaceVelocity.normalized;

                Vector3d courseCorrection = core.landing.ComputeCourseCorrection(false);
                double   correctionAngle  = courseCorrection.magnitude / (2.0 * vesselState.limitedMaxThrustAccel);

                correctionAngle     = Math.Min(0.1, correctionAngle);
                desiredThrustVector = (desiredThrustVector + correctionAngle * courseCorrection.normalized).normalized;

                if (Vector3d.Dot(vesselState.surfaceVelocity, vesselState.up) > 0 ||
                    Vector3d.Dot(vesselState.forward, desiredThrustVector) < 0.75)
                {
                    core.thrust.targetThrottle = (float)core.landing.minThrust;
                    status = "Braking (wrongdir)";
                }
                else
                {
                    double       controlledSpeed             = vesselState.speedSurface * Math.Sign(Vector3d.Dot(vesselState.surfaceVelocity, vesselState.up)); //positive if we are ascending, negative if descending
                    double       desiredSpeed                = -core.landing.MaxAllowedSpeed();
                    double       desiredSpeedAfterDt         = -core.landing.MaxAllowedSpeedAfterDt(vesselState.deltaT);
                    double       minAccel                    = -vesselState.localg * Math.Abs(Vector3d.Dot(vesselState.surfaceVelocity.normalized, vesselState.up));
                    double       maxAccel                    = vesselState.maxThrustAccel * Vector3d.Dot(vesselState.forward, -vesselState.surfaceVelocity.normalized) - vesselState.localg * Math.Abs(Vector3d.Dot(vesselState.surfaceVelocity.normalized, vesselState.up));
                    const double speedCorrectionTimeConstant = 0.3;
                    double       speedError                  = desiredSpeed - controlledSpeed;
                    double       desiredAccel                = speedError / speedCorrectionTimeConstant + (desiredSpeedAfterDt - desiredSpeed) / vesselState.deltaT;
                    if (maxAccel - minAccel > 0)
                    {
                        core.thrust.targetThrottle = Mathf.Clamp((float)((desiredAccel - minAccel) / (maxAccel - minAccel)), (float)core.landing.minThrust, 1.0F);
                    }
                    else
                    {
                        core.thrust.targetThrottle = (float)core.landing.minThrust;
                    }
                    status = "Braking: target speed = " + Math.Abs(desiredSpeed).ToString("F1") + " m/s";
                }

                core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, core.landing);

                return(this);
            }
            public override AutopilotStep OnFixedUpdate()
            {
                //if we don't want to deorbit but we're already on a reentry trajectory, we can't wait until the ideal point
                //in the orbit to deorbt; we already have deorbited.
                if (orbit.ApA < mainBody.RealMaxAtmosphereAltitude())
                {
                    core.thrust.targetThrottle = 0;
                    return(new CourseCorrection(core));
                }

                //We aim for a trajectory that
                // a) has the same vertical speed as our current trajectory
                // b) has a horizontal speed that will give it a periapsis of -10% of the body's radius
                // c) has a heading that points toward where the target will be at the end of free-fall, accounting for planetary rotation
                Vector3d   horizontalDV                  = OrbitalManeuverCalculator.DeltaVToChangePeriapsis(orbit, vesselState.time, 0.9 * mainBody.Radius);                //Imagine we are going to deorbit now. Find the burn that would lower our periapsis to -10% of the planet's radius
                Orbit      forwardDeorbitTrajectory      = orbit.PerturbedOrbit(vesselState.time, horizontalDV);                                                             //Compute the orbit that would put us on
                double     freefallTime                  = forwardDeorbitTrajectory.NextTimeOfRadius(vesselState.time, mainBody.Radius) - vesselState.time;                  //Find how long that orbit would take to impact the ground
                double     planetRotationDuringFreefall  = 360 * freefallTime / mainBody.rotationPeriod;                                                                     //Find how many degrees the planet will rotate during that time
                Vector3d   currentTargetRadialVector     = mainBody.GetWorldSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0) - mainBody.position; //Find the current vector from the planet center to the target landing site
                Quaternion freefallPlanetRotation        = Quaternion.AngleAxis((float)planetRotationDuringFreefall, mainBody.angularVelocity);                              //Construct a quaternion representing the rotation of the planet found above
                Vector3d   freefallEndTargetRadialVector = freefallPlanetRotation * currentTargetRadialVector;                                                               //Use this quaternion to find what the vector from the planet center to the target will be when we hit the ground
                Vector3d   freefallEndTargetPosition     = mainBody.position + freefallEndTargetRadialVector;                                                                //Then find the actual position of the target at that time
                Vector3d   freefallEndHorizontalToTarget = Vector3d.Exclude(vesselState.up, freefallEndTargetPosition - vesselState.CoM).normalized;                         //Find a horizontal unit vector that points toward where the target will be when we hit the ground
                Vector3d   currentHorizontalVelocity     = Vector3d.Exclude(vesselState.up, vesselState.orbitalVelocity);                                                    //Find our current horizontal velocity
                double     finalHorizontalSpeed          = (currentHorizontalVelocity + horizontalDV).magnitude;                                                             //Find the desired horizontal speed after the deorbit burn
                Vector3d   finalHorizontalVelocity       = finalHorizontalSpeed * freefallEndHorizontalToTarget;                                                             //Combine the desired speed and direction to get the desired velocity after the deorbi burn

                //Compute the angle between the location of the target at the end of freefall and the normal to our orbit:
                Vector3d currentRadialVector      = vesselState.CoM - mainBody.position;
                double   targetAngleToOrbitNormal = Vector3d.Angle(orbit.SwappedOrbitNormal(), freefallEndTargetRadialVector);

                targetAngleToOrbitNormal = Math.Min(targetAngleToOrbitNormal, 180 - targetAngleToOrbitNormal);

                double targetAheadAngle = Vector3d.Angle(currentRadialVector, freefallEndTargetRadialVector);       //How far ahead the target is, in degrees
                double planeChangeAngle = Vector3d.Angle(currentHorizontalVelocity, freefallEndHorizontalToTarget); //The plane change required to get onto the deorbit trajectory, in degrees

                //If the target is basically almost normal to our orbit, it doesn't matter when we deorbit; might as well do it now
                //Otherwise, wait until the target is ahead
                if (targetAngleToOrbitNormal < 10 ||
                    (targetAheadAngle < 90 && targetAheadAngle > 60 && planeChangeAngle < 90))
                {
                    deorbitBurnTriggered = true;
                }

                if (deorbitBurnTriggered)
                {
                    if (!MuUtils.PhysicsRunning())
                    {
                        core.warp.MinimumWarp();
                    }                                                           //get out of warp

                    Vector3d deltaV = finalHorizontalVelocity - currentHorizontalVelocity;
                    core.attitude.attitudeTo(deltaV.normalized, AttitudeReference.INERTIAL, core.landing);

                    if (deltaV.magnitude < 2.0)
                    {
                        return(new CourseCorrection(core));
                    }

                    status = "Doing high deorbit burn";
                }
                else
                {
                    core.attitude.attitudeTo(Vector3d.back, AttitudeReference.ORBIT, core.landing);
                    if (core.node.autowarp)
                    {
                        core.warp.WarpRegularAtRate((float)(orbit.period / 10));
                    }

                    status = "Moving to high deorbit burn point";
                }

                return(this);
            }
Example #18
0
        public override void GUI()
        {
            GUILayout.BeginHorizontal();
            useStoppingTime      = GUILayout.Toggle(useStoppingTime, "Maximum Stopping Time", GUILayout.ExpandWidth(false));
            maxStoppingTime.text = GUILayout.TextField(maxStoppingTime.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            useFlipTime      = GUILayout.Toggle(useFlipTime, "Minimum Flip Time", GUILayout.ExpandWidth(false));
            minFlipTime.text = GUILayout.TextField(minFlipTime.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
            GUILayout.EndHorizontal();

            if (!useStoppingTime)
            {
                useFlipTime = false;
            }

            GUILayout.BeginHorizontal();
            useControlRange       = GUILayout.Toggle(useControlRange, Localizer.Format("#MechJeb_HybridController_checkbox2"), GUILayout.ExpandWidth(false));//"RollControlRange"
            rollControlRange.text = GUILayout.TextField(rollControlRange.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("LD", GUILayout.ExpandWidth(false));
            LD.text = GUILayout.TextField(LD.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("Kp", GUILayout.ExpandWidth(false));
            Kp.text = GUILayout.TextField(Kp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("Ki", GUILayout.ExpandWidth(false));
            Ki.text = GUILayout.TextField(Ki.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("Kd", GUILayout.ExpandWidth(false));
            Kd.text = GUILayout.TextField(Kd.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
            GUILayout.EndHorizontal();

            // Not used yet
            //GuiUtils.SimpleTextBox("Maximum Relative Angular Velocity", kWlimit, "%");
            //double tmp_kWlimit = kWlimit;
            //tmp_kWlimit = (EditableDouble)GUILayout.HorizontalSlider((float)tmp_kWlimit, 0.0F, 1.0F);
            //
            //const int sliderPrecision = 3;
            //if (Math.Round(Math.Abs(tmp_kWlimit - kWlimit), sliderPrecision) > 0)
            //{
            //    kWlimit.val = Math.Round(tmp_kWlimit, sliderPrecision);
            //}

            GUILayout.BeginHorizontal();
            GUILayout.Label(Localizer.Format("#MechJeb_HybridController_label2"), GUILayout.ExpandWidth(true));//"Actuation"
            GUILayout.Label(MuUtils.PrettyPrint(Actuation), GUILayout.ExpandWidth(false));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("Error", GUILayout.ExpandWidth(true));
            GUILayout.Label(MuUtils.PrettyPrint(errorVector), GUILayout.ExpandWidth(false));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("Omega", GUILayout.ExpandWidth(true));
            GUILayout.Label(MuUtils.PrettyPrint(Omega), GUILayout.ExpandWidth(false));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("MaxOmega", GUILayout.ExpandWidth(true));
            GUILayout.Label(MuUtils.PrettyPrint(MaxOmega), GUILayout.ExpandWidth(false));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("TargetOmega", GUILayout.ExpandWidth(true));
            GUILayout.Label(MuUtils.PrettyPrint(TargetOmega), GUILayout.ExpandWidth(false));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label(Localizer.Format("#MechJeb_HybridController_label4"), GUILayout.ExpandWidth(true));//"TargetTorque"
            GUILayout.Label(MuUtils.PrettyPrint(TargetTorque), GUILayout.ExpandWidth(false));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label(Localizer.Format("#MechJeb_HybridController_label5"), GUILayout.ExpandWidth(true));//"ControlTorque"
            GUILayout.Label(MuUtils.PrettyPrint(ControlTorque), GUILayout.ExpandWidth(false));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("MaxAlpha", GUILayout.ExpandWidth(true));
            GUILayout.Label(MuUtils.PrettyPrint(MaxAlpha), GUILayout.ExpandWidth(false));
            GUILayout.EndHorizontal();
        }
Example #19
0
            void descend()
            {
                Quaternion courseCorrection = Quaternion.identity;

                // we aim for parachute break point with half deploy time for opening, which we can anticipate
                breakDiff = targetInfo.backwardDifference - (core.landing.deployChutes ? 0.5D * parachuteInfo.undeployedDistance : 0);

                status = String.Format("Holding planned descent attitude, break diff={0:F1}", breakDiff);

                String logs = String.Format("Atmo Correction alt:{0:F0}, speed hor:{1:F0}, AoA: {2:F1}, dist:{3:F0}", vesselState.altitudeASL.value, vesselState.speedSurfaceHorizontal.value, vesselState.AoA.value, targetInfo.forwardDistance);


                double backwardCorrection = breakDiff / targetInfo.forwardDistance;

                logs += String.Format(", target back:{0:F0} corr:{1:F4} break:{2:F0}", targetInfo.backwardDifference, backwardCorrection, breakDiff);


                if (targetInfo.isValid)
                {
                    double AoA = TrajectoriesConnector.API.AoA.Value;
                    //if we are less than 0°-60° turned on entry or 0-30° otherwise, simply turn more or less to hit target
                    if (!TrajectoriesConnector.API.isBelowEntry() && Math.Abs(backwardCorrection) > 0.02)
                    {
                        AoA = MuUtils.Clamp(AoA - Math.Sign(backwardCorrection) * 0.5, 120d, 180d, out aeroClamp, out _);
                        if (Math.Abs(TrajectoriesConnector.API.AoA.Value - AoA) > 0.1)
                        {
                            TrajectoriesConnector.API.AoA = AoA;
                            logs += String.Format(", changed AoA=>{0:F1}", AoA);
                        }
                    }
                    else if (Math.Abs(backwardCorrection) > 0.02)
                    {
                        AoA = MuUtils.Clamp(AoA - Math.Sign(backwardCorrection) * 0.5, 150d, 180d, out aeroClamp, out _);
                        if (TrajectoriesConnector.API.AoA != AoA)
                        {
                            TrajectoriesConnector.API.AoA = AoA;
                            logs += String.Format(", changed AoA=>{0:F1}", AoA);
                        }
                    }
                    if (aeroClamp && (core.thrust.targetThrottle != 0 || backwardCorrection > 0.03))
                    {
                        core.thrust.targetThrottle = Mathf.Clamp01((float)(gee / vesselState.limitedMaxThrustAccel * backwardCorrection)); // set thrust at 1G for 10% over target => early corrections, but slow enough for Trajectories
                        logs   += String.Format(", reverse thrust=>{0:P0}", core.thrust.targetThrottle);
                        status += ", +THRUST";
                    }
                    else if (!aeroClamp)
                    {
                        core.thrust.targetThrottle = 0; // safeguard, actually backwardCorrection should be 0 before Angle gets reduced
                    }
                }

                float rollForTarget = 0;

                // roll plannedOrientation towards target if we have signifikant lift
                if (vesselState.lift > 1)
                {
                    rollForTarget = -Mathf.Asin((float)(2.0 * targetInfo.normalDifference / targetInfo.distanceTarget.magnitude
                                                        * vesselState.mass * vesselState.speedSurface / (targetInfo.TimeTillImpact * vesselState.lift))) * Mathf.Rad2Deg;
                    rollForTarget = Mathf.Clamp(rollForTarget, -30, 30);
                }

                logs += String.Format(", normalDiff: {0:F0}, time: {1:F0}s, rollAngle={2:F1}", targetInfo.normalDifference, targetInfo.TimeTillImpact, rollForTarget);
                if (Mathf.Abs(rollForTarget) < 0.0005 || targetInfo.normalDifference < 50)
                {
                    rollForTarget = 0;                                                                         //neglect very small differences
                }
                if (Mathf.Abs(rollForTarget) > 0 && TrajectoriesConnector.API.isBelowEntry())
                {
                    status          += String.Format(", roll towards target with {0:F1}", rollForTarget);
                    courseCorrection = Quaternion.AngleAxis(rollForTarget, Vector3.forward);
                }
                Debug.Log(logs);

                Quaternion plannedOrientation = (Quaternion)TrajectoriesConnector.API.PlannedOrientation();

                core.attitude.attitudeTo(courseCorrection * plannedOrientation, AttitudeReference.SURFACE_VELOCITY, this);
            }
Example #20
0
        public override void GUI()
        {
            if (GUILayout.Button("Reset"))
            {
                ResetConfig();
            }

            Tf_autoTune = GUILayout.Toggle(Tf_autoTune, " Auto-tuning");

            GUILayout.BeginHorizontal();
            GUILayout.Space(20);
            GUILayout.BeginVertical();

            if (!Tf_autoTune)
            {
                GUILayout.Label("Larger ship do better with a larger Tf");

                GUILayout.BeginHorizontal();
                GUILayout.Label("Tf (s)", GUILayout.ExpandWidth(true));
                GUILayout.Label("P", GUILayout.ExpandWidth(false));
                UI_TfX.text = GUILayout.TextField(UI_TfX.text, GUILayout.ExpandWidth(true), GUILayout.Width(40));
                GUILayout.Label("Y", GUILayout.ExpandWidth(false));
                UI_TfY.text = GUILayout.TextField(UI_TfY.text, GUILayout.ExpandWidth(true), GUILayout.Width(40));
                GUILayout.Label("R", GUILayout.ExpandWidth(false));
                UI_TfZ.text = GUILayout.TextField(UI_TfZ.text, GUILayout.ExpandWidth(true), GUILayout.Width(40));
                GUILayout.EndHorizontal();

                UI_TfX = Math.Max(0.01, UI_TfX);
                UI_TfY = Math.Max(0.01, UI_TfY);
                UI_TfZ = Math.Max(0.01, UI_TfZ);
            }
            else
            {
                GUILayout.BeginHorizontal();
                GUILayout.Label("Tf", GUILayout.ExpandWidth(true));
                GUILayout.Label(MuUtils.PrettyPrint(TfV), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label("Tf range", GUILayout.ExpandWidth(true));
                GuiUtils.SimpleTextBox("min", UI_TfMin, "", 50);
                UI_TfMin = Math.Max(UI_TfMin, 0.01);
                GuiUtils.SimpleTextBox("max", UI_TfMax, "", 50);
                UI_TfMax = Math.Max(UI_TfMax, 0.01);
                GUILayout.EndHorizontal();
            }
            GUILayout.EndVertical();
            GUILayout.EndHorizontal();

            bool newLowPassFilter = GUILayout.Toggle(lowPassFilter, " Low Pass Filter");

            if (lowPassFilter != newLowPassFilter)
            {
                setPIDParameters();
                lowPassFilter = newLowPassFilter;
            }

            GUILayout.Label("PID factors");
            GuiUtils.SimpleTextBox("Kd = ", UI_kdFactor, " / Tf", 50);
            UI_kdFactor = Math.Max(UI_kdFactor, 0.01);
            GuiUtils.SimpleTextBox("Kp = pid.Kd / (", UI_kpFactor, " * Math.Sqrt(2) * Tf)", 50);
            UI_kpFactor = Math.Max(UI_kpFactor, 0.01);
            GuiUtils.SimpleTextBox("Ki = pid.Kp / (", UI_kiFactor, " * Math.Sqrt(2) * Tf)", 50);
            UI_kiFactor = Math.Max(UI_kiFactor, 0.01);
            GuiUtils.SimpleTextBox("Deadband = ", UI_deadband, "", 50);
            deadband = Math.Max(UI_deadband, 0.0);

            GuiUtils.SimpleTextBox("Maximum Relative Angular Velocity", kWlimit, "%");
            double tmp_kWlimit = kWlimit;

            tmp_kWlimit = (EditableDouble)GUILayout.HorizontalSlider((float)tmp_kWlimit, 0.0F, 1.0F);

            const int sliderPrecision = 3;

            if (Math.Round(Math.Abs(tmp_kWlimit - kWlimit), sliderPrecision) > 0)
            {
                kWlimit = Math.Round(tmp_kWlimit, sliderPrecision);
            }

            //showInfos = GUILayout.Toggle(showInfos, "Show Numbers");
            //if (showInfos)
            {
                GUILayout.BeginHorizontal();
                GUILayout.Label("Kp", GUILayout.ExpandWidth(true));
                GUILayout.Label(MuUtils.PrettyPrint(pid.Kp), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label("Ki", GUILayout.ExpandWidth(true));
                GUILayout.Label(MuUtils.PrettyPrint(pid.Ki), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label("Kd", GUILayout.ExpandWidth(true));
                GUILayout.Label(MuUtils.PrettyPrint(pid.Kd), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label("Error", GUILayout.ExpandWidth(true));
                GUILayout.Label(MuUtils.PrettyPrint(error * Mathf.Rad2Deg), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label("prop. action.", GUILayout.ExpandWidth(true));
                GUILayout.Label(MuUtils.PrettyPrint(pid.propAct), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label("deriv. action", GUILayout.ExpandWidth(true));
                GUILayout.Label(MuUtils.PrettyPrint(pid.derivativeAct), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label("integral action.", GUILayout.ExpandWidth(true));
                GUILayout.Label(MuUtils.PrettyPrint(pid.intAccum), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label("PID Action", GUILayout.ExpandWidth(true));
                GUILayout.Label(MuUtils.PrettyPrint(pidAction), GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label("Inertia", GUILayout.ExpandWidth(true));
                GUILayout.Label("|" + ac.inertia.magnitude.ToString("F3") + "| " + MuUtils.PrettyPrint(ac.inertia),
                                GUILayout.ExpandWidth(false));
                GUILayout.EndHorizontal();
            }

            if (!Tf_autoTune)
            {
                if (TfV.x != UI_TfX || TfV.y != UI_TfY || TfV.z != UI_TfZ)
                {
                    TfV.x = UI_TfX;
                    TfV.y = UI_TfY;
                    TfV.z = UI_TfZ;
                    setPIDParameters();
                }
            }
            else
            {
                if (TfMin != UI_TfMin || TfMax != UI_TfMax)
                {
                    TfMin = UI_TfMin;
                    TfMax = UI_TfMax;
                    setPIDParameters();
                }
            }
            if (kpFactor != UI_kpFactor || kiFactor != UI_kiFactor || kdFactor != UI_kdFactor)
            {
                kpFactor = UI_kpFactor;
                kiFactor = UI_kiFactor;
                kdFactor = UI_kdFactor;
                setPIDParameters();
            }
        }