void SetTargetHere()
        {
            Vessel vessel = FlightGlobals.ActiveVessel;

            if (vessel)
            {
                FlightGlobals.ActiveVessel.mainBody.GetLatLonAlt(vessel.GetWorldPos3D(), out double pickLat, out double pickLon, out double pickAlt);
                double lowestY = KSPUtils.FindLowestPointOnVessel(FlightGlobals.ActiveVessel);
                tgtLatitude  = pickLat;
                tgtLongitude = pickLon;
                tgtAlt       = (int)(pickAlt + lowestY);
                core.SetTarget(tgtLatitude, tgtLongitude, tgtAlt);
                core.Changed();
                GuiUtils.ScreenMessage(Localizer.Format("#BoosterGuidance_TargetSetToVessel"));
            }
        }
Esempio n. 2
0
 public static void SetActiveEngines(Vessel vessel, List <ModuleEngines> active)
 {
     foreach (var engine in KSPUtils.GetAllEngines(vessel))
     {
         if (active.Contains(engine))
         {
             if (!engine.isOperational)
             {
                 engine.Activate();
             }
         }
         else
         {
             if (engine.isOperational)
             {
                 engine.Shutdown();
             }
         }
     }
 }
 public void SetLandingBurnEnginesFromString(string s)
 {
     if (vessel == null)
     {
         return;
     }
     if (s == "current")
     {
         landingBurnEngines = null;
     }
     else
     {
         string[]             flags   = s.Split(',');
         List <ModuleEngines> engines = KSPUtils.GetAllEngines(vessel);
         if (flags.Length != engines.Count)
         {
             Debug.Log("[BoosterGuidance] Vessel " + vessel.name + " has " + engines.Count + " but landing burn engines list has length " + flags.Length);
             landingBurnEngines    = null;
             setLandingEnginesDone = false;
             return;
         }
         landingBurnEngines = new List <ModuleEngines>();
         for (int i = 0; i < flags.Length; i++)
         {
             if (flags[i] == "1")
             {
                 landingBurnEngines.Add(engines[i]);
             }
             else if (flags[i] != "0")
             {
                 Debug.Log("[BoosterGuidance] Found invalid string '" + s + "' for landingBurnEngines. Expected a boolean list of active engines. e.g. 0,0,1,1,0 or current");
             }
         }
     }
     setLandingEnginesDone = false;
 }
Esempio n. 4
0
        static public Vector3d ToGround(double tgtAlt, Vessel vessel, Trajectories.VesselAerodynamicModel aeroModel, CelestialBody body, BLController controller,
                                        Vector3d tgt_r, out double T, string logFilename = "", Transform logTransform = null, double timeOffset = 0, double maxT = 600)
        // Changes step size, dt, based on the amount of deacceleration forces, aero or thrust and winds back to choose smaller timesteps
        {
            float      ang;
            Quaternion bodyRotation;

            System.IO.StreamWriter f = null;
            if (logFilename != "")
            {
                f = new System.IO.StreamWriter(logFilename);
                f.WriteLine("time x y z vx vy vz ax ay az att_err target_error total_mass");
                f.WriteLine("# tgtAlt=" + tgtAlt);
            }

            T = 0;
            Vector3d          r = vessel.GetWorldPos3D() - body.position;
            Vector3d          v = vessel.GetObtVelocity();
            Vector3d          a = Vector3d.zero;
            Vector3d          last_r = r;
            Vector3d          last_v = v;
            BLControllerPhase last_phase = controller.phase;
            double            minThrust, maxThrust;
            double            totalMass = vessel.totalMass;

            // Initially thrust is for all operational engines
            KSPUtils.ComputeMinMaxThrust(vessel, out minThrust, out maxThrust);
            double y = r.magnitude - body.Radius;
            // TODO: att should be supplied as vessel transform will be wrong in simulation
            Vector3d att         = new Vector3d(vessel.transform.up.x, vessel.transform.up.y, vessel.transform.up.z);
            double   targetError = 0;

            if (controller != null)
            {
                // Take target error from previously calculated trajectory
                // We would know this at the end but can't wait until then
                targetError = controller.targetError;
            }

            // Use small dt all the way when below 5000m
            double dt_max = (y > 5000) ? dt_space : 1;
            double dt     = dt_max;
            double last_T = T;

            while ((y > tgtAlt) && (T < maxT))
            {
                y = r.magnitude - body.Radius;
                double dy = (r + v * dt).magnitude - body.Radius - y;

                if (y + dy < controller.reentryBurnAlt)
                {
                    dt = dt_reentry;
                }

                if ((controller.phase == BLControllerPhase.AeroDescent) || (controller.phase == BLControllerPhase.LandingBurn))
                {
                    dt = Math.Min(dt_aero, dt_max);
                }

                if (f != null)
                {
                    // NOTE: Cancel out rotation of planet
                    ang = (float)((-T) * body.angularVelocity.magnitude / Math.PI * 180.0);
                    // Rotation 1 second earlier
                    float prevang = (float)((-(T - 1)) * body.angularVelocity.magnitude / Math.PI * 180.0);
                    // Consider body rotation at this time
                    bodyRotation = Quaternion.AngleAxis(ang, body.angularVelocity.normalized);
                    Quaternion prevbodyRotation = Quaternion.AngleAxis(prevang, body.angularVelocity.normalized);
                    Vector3d   tr  = bodyRotation * r;
                    Vector3d   tr1 = prevbodyRotation * r;
                    Vector3d   tr2 = bodyRotation * (r + v);
                    Vector3d   ta  = bodyRotation * a;
                    tr = logTransform.InverseTransformPoint(tr + body.position);
                    Vector3d tv = logTransform.InverseTransformVector(tr2 - tr1);
                    ta = logTransform.InverseTransformVector(ta);
                    f.WriteLine(string.Format("{0} {1:F5} {2:F5} {3:F5} {4:F5} {5:F5} {6:F5} {7:F1} {8:F1} {9:F1} 0 {10:F2} {11:F2}", T + timeOffset, tr.x, tr.y, tr.z, tv.x, tv.y, tv.z, ta.x, ta.y, ta.z, targetError, totalMass));
                }

                if ((y < body.atmosphereDepth) || (y < controller.reentryBurnAlt + 1500 * dt))
                {
                    dt = Math.Min(dt, 2);
                }

                Vector3d vel_air;
                Vector3d steer;
                double   throttle;
                double   aeroFudgeFactor = 1.05; // Assume aero forces 5% higher which causes overshoot of target and more vertical final descent
                Vector3d out_r;
                Vector3d out_v;
                // Compute time step change in r and v
                EulerStep(dt, vessel, r, v, att, totalMass, minThrust, maxThrust, aeroModel, body, T, controller, tgt_r, aeroFudgeFactor, out steer, out vel_air, out throttle, out out_r, out out_v);

                y = r.magnitude - body.Radius;

                att = steer; // assume can turn immediately

                last_phase = controller.phase;
                last_r     = r;
                last_v     = v;
                last_T     = T;
                r          = out_r;
                v          = out_v;

                T = T + dt;
            }
            if (T > maxT)
            {
                Debug.Log("[BoosterGuidance] Simulation time exceeds maxT=" + maxT);
            }

            // Correct to point of intersect on surface
            double vy = Vector3d.Dot(last_v, Vector3d.Normalize(r));
            double p  = 0;

            if (vy < -0.1)
            {
                p = (tgtAlt - y) / -vy; // Backup proportion
                r = r - last_v * p;
                T = T - p;
            }
            if (f != null)
            {
                f.Close();
            }

            // Compensate for body rotation giving world position in the surface point now
            // that would be hit in the future
            ang          = (float)((-T) * body.angularVelocity.magnitude / Math.PI * 180.0);
            bodyRotation = Quaternion.AngleAxis(ang, body.angularVelocity.normalized);
            r            = bodyRotation * r;
            return(r);
        }
        public void Fly(FlightCtrlState state)
        {
            double   throttle = last_throttle;
            Vector3d steer    = last_steer;
            double   minThrust;
            double   maxThrust;

            KSPUtils.ComputeMinMaxThrust(vessel, out minThrust, out maxThrust);

            Vector3d tgt_r = vessel.mainBody.GetWorldSurfacePosition(tgtLatitude, tgtLongitude, tgtAlt);

            bool   landingGear        = false;
            bool   bailOutLandingBurn = true; // cut thrust if near ground and have too much thrust to reach ground
            double dt  = vessel.missionTime - last_t;
            string msg = "";

            if ((dt > 0.1) || (vessel.altitude < tgtAlt + 500))
            {
                msg = controller.GetControlOutputs(vessel, vessel.GetTotalMass(), vessel.GetWorldPos3D() - vessel.mainBody.position, vessel.GetObtVelocity(), vessel.transform.up, minThrust, maxThrust,
                                                   controller.vessel.missionTime, vessel.mainBody, false, out throttle, out steer, out landingGear, bailOutLandingBurn, debug);
                last_throttle = throttle;
                last_steer    = steer;
                last_t        = vessel.missionTime;
            }

            if ((landingGear) && (!reportedLandingGear))
            {
                KSPUtils.DeployLandingGear(vessel);
                if (vessel == FlightGlobals.ActiveVessel)
                {
                    GuiUtils.ScreenMessage(Localizer.Format("#BoosterGuidance_DeployingLandingGear"));
                }
            }

            if ((msg != "") && (vessel == FlightGlobals.ActiveVessel))
            {
                GuiUtils.ScreenMessage(msg);
            }

            if (vessel.checkLanded())
            {
                DisableGuidance();
                state.mainThrottle = 0;
                return;
            }

            // Set active engines in landing burn
            if (controller.phase == BLControllerPhase.LandingBurn)
            {
                if (controller.landingBurnEngines != null)
                {
                    foreach (ModuleEngines engine in KSPUtils.GetAllEngines(vessel))
                    {
                        if (controller.landingBurnEngines.Contains(engine))
                        {
                            if (!engine.isOperational)
                            {
                                engine.Activate();
                            }
                        }
                        else
                        {
                            if (engine.isOperational)
                            {
                                engine.Shutdown();
                            }
                        }
                    }
                }
            }

            // Draw predicted position if controlling that vessel
            if (vessel == FlightGlobals.ActiveVessel)
            {
                double lat, lon, alt;
                // prediction is for position of planet at current time compensating for
                // planet rotation
                vessel.mainBody.GetLatLonAlt(controller.predBodyRelPos + controller.vessel.mainBody.position, out lat, out lon, out alt);
                alt = vessel.mainBody.TerrainAltitude(lat, lon);              // Make on surface
                Targets.RedrawPrediction(vessel.mainBody, lat, lon, alt + 1); // 1m above ground

                Targets.DrawSteer(vessel.vesselSize.x * Vector3d.Normalize(steer), null, Color.green);
            }
            state.mainThrottle = (float)throttle;
            vessel.Autopilot.SAS.lockedMode = false;
            vessel.Autopilot.SAS.SetTargetOrientation(steer, false);
        }
        public override string GetControlOutputs(
            Vessel vessel,
            double totalMass,
            Vector3d r,         // world pos relative to body
            Vector3d v,         // world velocity
            Vector3d att,       // attitude
            double minThrust, double maxThrust,
            double t,
            CelestialBody body,
            bool simulate,                   // if true just go retrograde (no corrections)
            out double throttle, out Vector3d steer,
            out bool landingGear,            // true if landing gear requested (stays true)
            bool bailOutLandingBurn = false, // if set too true on RO set throttle=0 if thrust > gravity at landing
            bool showCpuTime        = false)

        {
            // height of lowest point with additional margin
            double            y           = r.magnitude - body.Radius - tgtAlt - touchdownMargin + lowestY;
            Vector3d          up          = Vector3d.Normalize(r);
            Vector3d          vel_air     = v - body.getRFrmVel(r + body.position);
            double            vy          = Vector3d.Dot(vel_air, up);
            double            amin        = minThrust / totalMass;
            double            amax        = maxThrust / totalMass;
            float             minThrottle = 0.01f;
            BLControllerPhase lastPhase   = phase;
            Vector3d          tgt_r       = body.GetWorldSurfacePosition(tgtLatitude, tgtLongitude, tgtAlt) - body.position;

            System.Diagnostics.Stopwatch timer = new System.Diagnostics.Stopwatch();
            timer.Start();
            string msg = ""; // return message about the status

            landingGear = (y < deployLandingGearHeight) && (deployLandingGear);

            double g  = FlightGlobals.getGeeForceAtPosition(r + body.position).magnitude;
            double dt = t - lastt;

            // No thrust - retrograde relative to surface (default and Coasting phase
            throttle = 0;
            steer    = -Vector3d.Normalize(vel_air);

            Vector3d error = Vector3d.zero;

            attitudeError = 0;
            if ((!simulate) && (y > noSteerHeight))
            {
                BLController tc = new BLController(this);
                // Only simulate phases beyond boostback so boostback minimizes error and simulate includes just
                // the remaining phases and doesn't try to redo reentry burn for instance
                if (phase == BLControllerPhase.BoostBack)
                {
                    tc.phase = BLControllerPhase.Coasting;
                }

                predBodyRelPos    = Simulate.ToGround(tgtAlt, vessel, aeroModel, body, tc, tgt_r, out targetT);
                landingBurnAMax   = tc.landingBurnAMax;
                landingBurnHeight = tc.landingBurnHeight;        // Update from simulation
                error             = predBodyRelPos - tgt_r;
                error             = Vector3d.Exclude(up, error); // make sure error is horizontal
                targetError       = error.magnitude;
            }

            // BOOSTBACK
            if (phase == BLControllerPhase.BoostBack)
            {
                // Aim to close max of 20% of error in 1 second
                steer = -Vector3d.Normalize(error);
                // Safety checks in inverse cosine
                attitudeError = HGUtils.angle_between(att, steer);
                double dv = error.magnitude / targetT; // estimated delta V needed
                double ba = 0;
                if (attitudeError < 10 + dv * 0.5)     // more accuracy needed when close to target
                {
                    ba = Math.Max(0.3 * dv, 10 / targetT);
                }
                throttle = Mathf.Clamp((float)((ba - amin) / (0.01 + amax - amin)), minThrottle, 1);
                // Stop if error has grown significantly
                if ((targetError > minError * 1.5) || (targetError < 10))
                {
                    if (targetError < 5000) // check if error changes dramatically but still far from target
                    {
                        phase = BLControllerPhase.Coasting;
                        msg   = Localizer.Format("#BoosterGuidance_SwitchedToCoasting");
                    }
                }
                minError = Math.Min(targetError, minError);
                if ((y < reentryBurnAlt) && (vy < 0)) // falling
                {
                    phase = BLControllerPhase.ReentryBurn;
                    msg   = Localizer.Format("#BoosterGuidance_SwitchedToReentryBurn");
                }

                // TODO - Check for steer in 180 degrees as interpolation wont work
                steer = Vector3d.Normalize(att * 0.75 + steer * 0.25); // simple interpolation to damp rapid oscillations
            }

            // COASTING
            if (phase == BLControllerPhase.Coasting)
            {
                if ((y < reentryBurnAlt) && (vy < 0))
                {
                    phase = BLControllerPhase.ReentryBurn;
                    msg   = Localizer.Format("#BoosterGuidance_SwitchedToReentryBurn");
                }
            }

            // Set default gains for steering
            steerGain = 0;

            // RE-ENTRY BURN
            if (phase == BLControllerPhase.ReentryBurn)
            {
                double errv = vel_air.magnitude - reentryBurnTargetSpeed;

                if (errv > 0)
                {
                    double smooth = HGUtils.LinearMap((double)y, (double)reentryBurnAlt, (double)reentryBurnAlt - 4000, 0, 1);
                    // Limit maximum de-acceleration to make the simulation accuracy when dt=2 or 4 secs
                    double da = g + Math.Min(Math.Max(errv * 0.3, 10), 50); // attempt to cancel 30% of extra velocity in 1 sec and min of 10m/s/s
                    // Use of dt prevents too high throttle when simulating re-entry burn with dt=2 or 4 secs.
                    double newThrottle = smooth * (da - amin) / (0.01 + amax - amin);
                    throttle = HGUtils.Clamp(newThrottle, minThrottle, 1);
                }
                else
                {
                    phase = BLControllerPhase.AeroDescent;
                    msg   = Localizer.Format("#BoosterGuidance_SwitchedToAeroDescent");
                }

                if (!simulate)
                {
                    pid_reentry.kp = reentryBurnSteerKp * CalculateSteerGain(throttle, vel_air, r, y, totalMass, false);
                    steerGain      = pid_reentry.kp;
                    double ang = pid_reentry.Update(error.magnitude, Time.deltaTime);
                    steer = -Vector3d.Normalize(vel_air) + GetSteerAdjust(error, ang, reentryBurnMaxAoA);
                }
            }

            // desired velocity - used in AERO DESCENT and LANDING BURN
            double dvy = -touchdownSpeed;
            double av  = Math.Max(0.1, landingBurnAMax - g);

            // AERO DESCENT
            if (phase == BLControllerPhase.AeroDescent)
            {
                if (!simulate)
                {
                    pid_aero.kp = aeroDescentSteerKp * CalculateSteerGain(0, vel_air, r, y, totalMass, false);
                    steerGain   = pid_aero.kp;
                    double ang = pid_aero.Update(error.magnitude, dt);
                    steer = -Vector3d.Normalize(vel_air) + GetSteerAdjust(error, ang, aeroDescentMaxAoA);
                }

                double landingMinThrust, landingMaxThrust;
                KSPUtils.ComputeMinMaxThrust(vessel, out landingMinThrust, out landingMaxThrust, false, landingBurnEngines);
                double newLandingBurnAMax = landingMaxThrust / totalMass;

                if (Math.Abs(landingBurnAMax - newLandingBurnAMax) > 0.5)
                {
                    landingBurnAMax   = landingMaxThrust / totalMass; // update so we don't continually recalc
                    landingBurnHeight = Simulate.CalculateLandingBurnHeight(tgtAlt, r, v, vessel, totalMass, landingMinThrust, landingMaxThrust, aeroModel, vessel.mainBody, this, 100, "", suicideFactor);
                }

                if (y - vel_air.magnitude * igniteDelay <= landingBurnHeight) // Switch to landing burn N secs earlier to allow RO engine start up time
                {
                    lowestY = KSPUtils.FindLowestPointOnVessel(vessel);
                    phase   = BLControllerPhase.LandingBurn;
                    msg     = Localizer.Format("#BoosterGuidance_SwitchedToLandingBurn");
                }
                // Interpolate to avoid rapid swings
                steer = Vector3d.Normalize(att * 0.75 + steer * 0.25); // simple interpolation to damp rapid oscillations
            }

            // LANDING BURN (suicide burn)
            if (phase == BLControllerPhase.LandingBurn)
            {
                if ((landingBurnEngines != null) && (!setLandingEnginesDone) && (!simulate))
                {
                    KSPUtils.SetActiveEngines(vessel, landingBurnEngines);
                    msg = string.Format(Localizer.Format("#BoosterGuidance_SetXEnginesForLanding", landingBurnEngines.Count.ToString()));
                    setLandingEnginesDone = true;
                }
                av = Math.Max(0.1, amax - g); // wrong on first iteration
                if (y > 0)
                {
                    dvy = -Math.Sqrt((1 + suicideFactor) * av * y) - touchdownSpeed; // Factor is 2 for perfect suicide burn, lower for margin and hor vel
                }
                if (amax > 0)
                {
                    double err_dv = vy - dvy;                // +ve is velocity too high
                    double da     = g - 0.3 * (err_dv / dt); // required accel to change vy in about the next three timesteps, cancel out g (only works if vertical)

                    throttle = HGUtils.Clamp((da - amin) / (0.01 + amax - amin), minThrottle, 1);

                    // compensate if not vertical as need more vertical component of thrust
                    throttle = HGUtils.Clamp(throttle / Math.Max(0.1, Vector3.Dot(att, up)), minThrottle, 1);
                }
                if ((!simulate) && (y > noSteerHeight))
                {
                    double ang;
                    pid_landing.kp = landingBurnSteerKp * CalculateSteerGain(throttle, vel_air, r, y, totalMass, false);
                    steerGain      = pid_landing.kp;
                    ang            = pid_landing.Update(error.magnitude, Time.deltaTime);
                    // Steer retrograde with added up component to damp oscillations at slow speed near ground
                    steer = -Vector3d.Normalize(vel_air - 20 * up) + GetSteerAdjust(error, ang, landingBurnMaxAoA);
                }
                else
                {
                    // Just cancel velocity with significant upwards component to stay upright
                    steer = -Vector3d.Normalize(vel_air - 20 * up);
                }
                if ((y < noSteerHeight) && (!noSteerReported))
                {
                    msg             = string.Format(Localizer.Format("#BoosterGuidance_NoSteerHeightReached"));
                    noSteerReported = true;
                }

                // Decide to shutdown engines for final touch down? (within 3 secs)
                // Criteria should be if
                // height
                double minHeight = KSPUtils.MinHeightAtMinThrust(y, vy, amin, g);
                // Criteria for shutting down engines
                // - we could not reach ground at minimum thrust (would ascend)
                // - falling less than touchdown speed (otherwise can decide to shutdown engines when still high and travelling fast)
                // This is particulary done to stop the simulation never hitting the ground and making pretty circles through the sky
                // until the maximum time is exceeded. The predicted impact position will vary widely and this was incur a lot of time to calculate
                // - this is very tricky to get right for the actual vessel since in RO engines take time to throttle down, so it needs to be done
                //   early, allowing for the fact the residual engine thrust will slow the rocket more for the next 2-3 secs
                // - the engine will restart again if landing doesn't happen with 2-3 secs
                bool cant_reach_ground = (minHeight > 0) && (vy > -50);
                if ((cant_reach_ground) && (bailOutLandingBurn))
                {
                    throttle = 0;
                }

                // Interpolate to avoid rapid swings
                steer = Vector3d.Normalize(att * 0.75 + steer * 0.25); // simple interpolation to damp rapid oscillations
            }

            // Logging
            if (fp != null)
            {
                Vector3d a  = att * (amin + throttle * (amax - amin)); // this assumes engine is ignited though
                Vector3d tr = logTransform.InverseTransformPoint(r + body.position);
                Vector3d tv = logTransform.InverseTransformVector(vel_air);
                Vector3d ta = logTransform.InverseTransformVector(a);
                fp.WriteLine("{0:F1} {1} {2:F1} {3:F1} {4:F1} {5:F1} {6:F1} {7:F1} {8:F1} {9:F1} {10:F1} {11:F1} {12:F1} {13:F1} {14:F3} {15:F1} {16:F2}", t - logStartTime, phase, tr.x, tr.y, tr.z, tv.x, tv.y, tv.z, a.x, a.y, a.z, attitudeError, amin, amax, steerGain, targetError, totalMass);
                logLastTime = t;
            }

            lastt         = t;
            steer         = Vector3d.Normalize(steer);
            attitudeError = HGUtils.angle_between(att, steer);

            throttle = HGUtils.Clamp(throttle, 0, 1);

            // Log simulate to ground when phase changes
            // So the logging is done at the start of the new phase
            if ((lastPhase != phase) && (fp != null))
            {
                LogSimulation();
            }

            elapsed_secs = timer.ElapsedMilliseconds * 0.001;

            // Set info message
            string tgtErrStr;

            if (targetError > 1000)
            {
                if (targetError > 100000)
                {
                    tgtErrStr = string.Format("{0:F0}km", targetError * 0.001);
                }
                else
                {
                    if (targetError > 10000)
                    {
                        tgtErrStr = string.Format("{0:F1}km", targetError * 0.001);
                    }
                    else
                    {
                        tgtErrStr = string.Format("{0:F2}km", targetError * 0.001);
                    }
                }
            }
            else
            {
                tgtErrStr = string.Format("{0:F0}m", targetError);
            }
            if (vessel.checkLanded())
            {
                info = string.Format(Localizer.Format("#BoosterGuidance_LandedXFromTarget", tgtErrStr));
            }
            else
            {
                string s1 = tgtErrStr;
                string s2 = string.Format("{0:F0}", attitudeError);
                string s3 = string.Format("{0:F0}", targetT);
                string s4 = string.Format("{0:F0}", elapsed_secs * 1000);
                if (showCpuTime)
                {
                    info = string.Format(Localizer.Format("#BoosterGuidance_ErrorXTimeXCPUX", s1, s2, s3, s4));
                }
                else
                {
                    info = string.Format(Localizer.Format("#BoosterGuidance_ErrorXTimeX", s1, s2, s3));
                }
            }

            if (msg != "")
            {
                info = msg;
            }

            return(msg);
        }
 public void AttachVessel(Vessel a_vessel, bool useFAR = false)
 {
     vessel    = a_vessel;
     aeroModel = Trajectories.AerodynamicModelFactory.GetModel(vessel, vessel.mainBody, useFAR);
     lowestY   = KSPUtils.FindLowestPointOnVessel(vessel);
 }