void Update() { NavWaypoint navWaypoint = NavWaypoint.fetch; //TODO optimize these searches if (navWaypoint.IsActive && CameraManager.Instance.currentCameraMode == CameraManager.CameraMode.IVA) { InternalNavBall navBall = null; foreach (InternalProp prop in CameraManager.Instance.IVACameraActiveKerbal.InPart.internalModel.props) { navBall = (InternalNavBall)prop.internalModules.Find(module => module.GetType().Equals(typeof(InternalNavBall))); if (navBall != null) { break; } } if (navBall != null) { if (originalIVANavBallWaypointSize.Equals(Vector3.zero)) { originalIVANavBallWaypointSize = navBall.navWaypointVector.localScale; } navBall.navWaypointVector.localScale = GlobalVariables.Settings.hideNavBallWaypoint ? Vector3.zero : originalIVANavBallWaypointSize; } } if (originalNavBallWaypointSize.Equals(Vector3.zero)) { originalNavBallWaypointSize = navWaypoint.Visual.transform.localScale; } navWaypoint.Visual.transform.localScale = GlobalVariables.Settings.hideNavBallWaypoint ? Vector3.zero : originalNavBallWaypointSize; }
//[KSPEvent(guiActive = true, guiName = "Set to active waypoint", isPersistent = true)] public void SetToWaypoint() { NavWaypoint navPoint = NavWaypoint.fetch; if (navPoint == null || !navPoint.IsActive || navPoint.Body != this.vessel.mainBody) { ScreenMessages.PostScreenMessage("No valid nav point"); } else { Deactivate(); double[] newCoordinates = GeoUtils.StepBack( this.vessel.latitude, this.vessel.longitude, navPoint.Latitude, navPoint.Longitude, this.vessel.mainBody.Radius, 200 ); this.targetLatitude = newCoordinates[0]; this.targetLongitude = newCoordinates[1]; this.distanceTravelled = 0; FindPath(); } }
public void OnParameterChange(Contract c, ContractParameter p) { if (c != Root) { return; } // Hide the waypoint if we are done with it if (hideOnCompletion && waypoint != null && waypoint.visible) { for (IContractParameterHost paramHost = this; paramHost != Root; paramHost = paramHost.Parent) { if (state == ParameterState.Complete) { ContractParameter param = paramHost as ContractParameter; if (param != null && !param.Enabled) { waypoint.visible = false; NavWaypoint navPoint = NavWaypoint.fetch; if (navPoint != null && NavWaypoint.fetch.IsActive && navPoint.Latitude == waypoint.latitude && navPoint.Longitude == waypoint.longitude) { NavWaypoint.fetch.Clear(); NavWaypoint.fetch.Deactivate(); } break; } } else { break; } } } }
static public void linkNavPoint() { if (HighLogic.LoadedSceneIsFlight) { if (navWaypoint == null) { navWaypoint = (GameObject.FindObjectOfType(typeof(NavWaypoint)) as NavWaypoint); } } }
/// <summary> /// Checks if the given waypoint is the nav waypoint. /// </summary> /// <param name="waypoint"></param> /// <returns></returns> public static bool IsNavPoint(Waypoint waypoint) { NavWaypoint navPoint = FinePrint.WaypointManager.navWaypoint; if (navPoint == null || !FinePrint.WaypointManager.navIsActive()) { return(false); } return(navPoint.latitude == waypoint.latitude && navPoint.longitude == waypoint.longitude); }
/// <summary> /// Checks if navigation waypoint is active. /// </summary> /// <returns>True or False.</returns> private bool NavWaypointIsActive() { if (!HighLogic.LoadedSceneIsFlight) { return(false); } NavWaypoint nav = (UnityEngine.Object.FindObjectOfType(typeof(NavWaypoint)) as NavWaypoint); return(nav != null && nav.IsActive); }
public static void updateNavigationData() { //see if information is current if (GetLastNavUpdateUT() != Planetarium.GetUniversalTime()) { selectedGlideSlope = gsList[gsIdx]; selectedRwy = rwyList[rwyIdx]; //Since there seems to be no callback methods to determine whether waypoint has been set or changed, we have to refresh INS data on every update NavWaypoint navpoint = FinePrint.WaypointManager.navWaypoint; if (FinePrint.WaypointManager.navIsActive() && (navpoint != null)) { //Trying to find the FinePrint waypoint that navigation is set for: Waypoint waypoint = null; foreach (Waypoint wp in FinePrint.WaypointManager.Instance().Waypoints) { if (navpoint.latitude == wp.latitude && navpoint.longitude == wp.longitude) { waypoint = wp; break; } } if (waypoint != null) { //If waypoint is fine then generate fake target runway every time Runway insTarget = new Runway(); insTarget.isINSTarget = true; insTarget.ident = waypoint.name; insTarget.hdg = selectedRwy != null ? selectedRwy.hdg : 0; insTarget.altMSL = (float)(waypoint.height + waypoint.altitude); insTarget.locLatitude = (float)navpoint.latitude; insTarget.locLongitude = (float)navpoint.longitude; insTarget.gsLatitude = (float)navpoint.latitude; insTarget.gsLongitude = (float)navpoint.longitude; selectedRwy = insTarget; } } currentVessel = FlightGlobals.ActiveVessel; bearing = NavUtilLib.Utils.CalcBearingToBeacon(currentVessel, selectedRwy); dme = NavUtilLib.Utils.CalcDistanceToBeacon(currentVessel, selectedRwy); elevationAngle = NavUtilLib.Utils.CalcElevationAngle(currentVessel, selectedRwy); //locDeviation = NavUtilLib.Utils.CalcLocalizerDeviation(bearing, selectedRwy); locDeviation = (float)NavUtilLib.Utils.CalcLocalizerDeviation(currentVessel, selectedRwy); gsDeviation = NavUtilLib.Utils.CalcGlideslopeDeviation(elevationAngle, selectedGlideSlope); // runwayHeading = (float)NavUtilLib.Utils.CalcProjectedRunwayHeading(currentVessel, selectedRwy); SetLastNavUpdateUT(); } }
/// <summary> /// Checks if the given waypoint is the nav waypoint. /// </summary> /// <param name="waypoint"></param> /// <returns></returns> public static bool IsNavPoint(Waypoint waypoint) { NavWaypoint navPoint = NavWaypoint.fetch; if (navPoint == null || !NavWaypoint.fetch.IsActive) { return(false); } return(navPoint.Latitude == waypoint.latitude && navPoint.Longitude == waypoint.longitude); }
static public bool navIsActive() { if (HighLogic.LoadedSceneIsFlight) { if (navWaypoint == null) { navWaypoint = (GameObject.FindObjectOfType(typeof(NavWaypoint)) as NavWaypoint); } if (navWaypoint != null) { return(navWaypoint.isActive()); } } return(false); }
/// <summary> /// Returns latitude and longitude of a waypoint or double.MinValue, if waypoint is not valid /// </summary> /// <param name="v"></param> /// <returns>[latitude, longitude]</returns> internal static double[] GetCurrentWaypointLatLon(Vessel v) { NavWaypoint waypoint = NavWaypoint.fetch; if ((waypoint == null) || !waypoint.IsActive || (waypoint.Body != v.mainBody)) { return new double[2] { double.MinValue, double.MinValue } } ; else { return new double[2] { waypoint.Latitude, waypoint.Longitude } }; }
void select_waypoint() { NavWaypoint navPoint = NavWaypoint.fetch; if (navPoint == null || !navPoint.IsActive || navPoint.Body != this.vessel.mainBody) { MessageManager.post_quick_message("No waypoint available"); } else { current_waypt.longitude = navPoint.Longitude; current_waypt.latitude = navPoint.Latitude; Debug.Log($"[AtmosphereAutopilot] waypoint lat {current_waypt.latitude} lon {current_waypt.longitude}"); desired_latitude.Value = (float)current_waypt.latitude; desired_longitude.Value = (float)current_waypt.longitude; AtmosphereAutopilot.Instance.mainMenuGUIUpdate(); WaypointMode = true; MessageManager.post_quick_message("Waypoint selected"); } }
/// <summary> /// Checks if navigation waypoint is active. /// </summary> /// <returns>True or False.</returns> private bool NavWaypointIsActive() { if (!HighLogic.LoadedSceneIsFlight) { return(false); } this.NavWaypointCheckCounter++; if (this.NavWaypointCheckCounter < 20) { return(this.IsNavWaypointActivePrevValue); } else { this.NavWaypointCheckCounter = 0; NavWaypoint nav = NavWaypoint.fetch; this.IsNavWaypointActivePrevValue = nav != null && nav.IsActive; return(this.IsNavWaypointActivePrevValue); } }
static public bool navIsActive() { if (HighLogic.LoadedSceneIsFlight) { if (navWaypoint == null) navWaypoint = (GameObject.FindObjectOfType(typeof(NavWaypoint)) as NavWaypoint); if (navWaypoint != null) { return navWaypoint.isActive(); } } return false; }
static public void linkNavPoint() { if (HighLogic.LoadedSceneIsFlight) { if (navWaypoint == null) navWaypoint = (GameObject.FindObjectOfType(typeof(NavWaypoint)) as NavWaypoint); } }
/// <summary> /// Whether this vessel meets the parameter condition. /// </summary> /// <param name="vessel">The vessel to check</param> /// <returns>Whether the vessel meets the condition</returns> protected override bool VesselMeetsCondition(Vessel vessel) { LoggingUtil.LogVerbose(this, "Checking VesselMeetsCondition: " + vessel.id); // Make sure we have a waypoint if (waypoint == null && Root != null) { waypoint = FetchWaypoint(Root, true); } if (waypoint == null) { return(false); } // Not even close if (vessel.mainBody.name != waypoint.celestialName) { return(false); } // Default distance if (distance == 0.0 && horizontalDistance == 0.0) { // Close to the surface if (waypoint.altitude < 25.0) { distance = 500.0; } else { distance = Math.Max(1000.0, waypoint.altitude / 5.0); } } // Calculate the distance bool check = false; if (distance != 0.0) { double actualDistance = WaypointUtil.GetDistanceToWaypoint(vessel, waypoint, ref height); LoggingUtil.LogVerbose(this, "Distance to waypoint '" + waypoint.name + "': " + actualDistance); check = actualDistance <= distance; } else { double actualDistance = WaypointUtil.GetDistance(vessel.latitude, vessel.longitude, waypoint.latitude, waypoint.longitude, vessel.altitude); LoggingUtil.LogVerbose(this, "Horizontal distance to waypoint '" + waypoint.name + "': " + actualDistance); check = actualDistance <= horizontalDistance; } // Output the message for entering/leaving the waypoint area. if (showMessages) { if (check ^ nearWaypoint) { nearWaypoint = check; string waypointName = waypoint.name + (waypoint.isClustered ? " " + StringUtilities.IntegerToGreek(waypoint.index) : ""); string msg = "You are " + (nearWaypoint ? "entering " : "leaving ") + waypointName + "."; ScreenMessages.PostScreenMessage(msg, 5.0f, ScreenMessageStyle.UPPER_CENTER); } NavWaypoint navWaypoint = NavWaypoint.fetch; if (navWaypoint != null && navWaypoint.Latitude == waypoint.latitude && navWaypoint.Longitude == waypoint.longitude) { navWaypoint.IsBlinking = nearWaypoint; } } return(check); }
//This method is called periodically by the main thread, //then it decides if it is time to start a new thread or not public static void update() { now = Time.time; //double lc = lastCharge; //float lt = lastTime; //lastTime = now; double dt = TimeWarp.fixedDeltaTime; //Debug.Log("Starting simulation at " + Math.Round(now, 3)); isReadable = false; //Reset some stuff ship = FlightGlobals.ActiveVessel; body = ship.mainBody; _gravity = FlightGlobals.getGeeForceAtPosition(ship.CoM).magnitude; _electricCharge = 0; _electricMax = 0; _evamaxfuel = 0; _evafuel = 0; _liquidFuel = 0; _liquidMax = 0; _monoFuel = 0; _monoMax = 0; _airin = 0; _airreq = 0; _mass = 0; _max_thrust = 0; _cur_thrust = 0; _isp = 0; _engineaccel = 0; _enginedecel = 0; _max_temp_percent = 0; _max_temp_actual = 0; double _best_temp = 0; double _best_ablation = 0; _ablation = 1; _tarpos = new Vector3(); // int cur_stage = ship.currentStage; //Debug.Log("##########################"); //Debug.Log("Current Stage: "+cur_stage); //Part loop - look at all the parts and modules for stuff. foreach (Part P in ship.parts) { /*Debug.Log("-------------------------------"); * Debug.Log(P.name); * Debug.Log("inStageIndex: " + P.inStageIndex); * Debug.Log("inv Stage: " + P.inverseStage); * Debug.Log("Manual Stage Offset: " + P.manualStageOffset); * Debug.Log("Computed Stage: " + (P.inverseStage + P.manualStageOffset)); * Debug.Log("Original Stage: " + P.originalStage); * //Debug.Log("Stage After: " + P.stageAfter); * //Debug.Log("Stage Before: " + P.stageBefore); * Debug.Log("Stage Offset: " + P.stageOffset); */ //Temperature //First, look at internal temps _best_temp = P.temperature / P.maxTemp; if (_best_temp > _max_temp_percent) { _max_temp_percent = _best_temp; _max_temp_actual = P.temperature; } //Now look at skin temps _best_temp = P.skinTemperature / P.skinMaxTemp; if (_best_temp > _max_temp_percent) { _max_temp_percent = _best_temp; _max_temp_actual = P.skinTemperature; } //if (P.temperature/P.maxTemp > .5) Debug.Log(P.partInfo.title + " Temp: " + (int)P.temperature + "⁰/" + (int)P.maxTemp + "⁰"); //Vessel Mass if (P.physicalSignificance == Part.PhysicalSignificance.FULL) { _mass += P.mass; _mass += P.GetResourceMass(); } //Resource loop foreach (PartResource pr in P.Resources) { if (pr.resourceName.Equals("ElectricCharge")) { _electricCharge += pr.amount; _electricMax += pr.maxAmount; } else if (pr.resourceName.Equals("LiquidFuel")) { _liquidFuel += pr.amount; _liquidMax += pr.maxAmount; } else if (pr.resourceName.Equals("MonoPropellant")) { _monoFuel += pr.amount; _monoMax += pr.maxAmount; } else if (pr.resourceName.Equals("Ablator")) { _best_ablation = pr.amount / pr.maxAmount; if (_best_ablation < _ablation) { _ablation = _best_ablation; } } } //Module loop foreach (PartModule pm in P.Modules) { if (pm is KerbalEVA) { KerbalEVA eva = pm as KerbalEVA; _evafuel = eva.Fuel; _evamaxfuel = eva.FuelCapacity; } else if (pm is ModuleWheelDeployment) { _gearstate = 0; //Gear up ModuleWheelDeployment mlg = pm as ModuleWheelDeployment; if (mlg.stateString.Equals("Deployed")) { _gearstate = 3; } if (mlg.stateString.StartsWith("Deploying")) { _gearstate = 2; } if (mlg.stateString.StartsWith("Retracting")) { _gearstate = 1; } } else if (pm is ModuleEngines) { ModuleEngines ME = pm as ModuleEngines; if (ME.engineShutdown || !ME.EngineIgnited) //We don't care about engines that aren't ready to fire { continue; } if (1 / ME.engineAccelerationSpeed > _engineaccel) { _engineaccel = 1 / ME.engineAccelerationSpeed; } if (1 / ME.engineDecelerationSpeed > _enginedecel) { _enginedecel = 1 / ME.engineDecelerationSpeed; } //Thrust/ISP float thrust = ME.maxThrust * ME.thrustPercentage * 0.01f; _isp += thrust / ME.atmosphereCurve.Evaluate((float)ship.staticPressurekPa); _max_thrust += thrust; _cur_thrust += ME.finalThrust; //I think this is the actual thrust being produced by each engine //Intake Air foreach (Propellant Pro in ME.propellants) { if (Pro.name.Equals("IntakeAir")) { _airreq += Pro.currentRequirement; } } //Overheat stuff //if (!ME.flameout) //{ // double temp = P.temperature / P.maxTemp; // if (temp > _max_temp) _max_temp = temp; //} } else if (pm is ModuleEnginesFX) //The newer engines all use this instead of ModuleEngines { ModuleEnginesFX MFX = pm as ModuleEnginesFX; if (MFX.engineShutdown || !MFX.EngineIgnited) { continue; } //Thrust/ISP float thrust = MFX.maxThrust * MFX.thrustPercentage * 0.01f; _isp += thrust / MFX.atmosphereCurve.Evaluate((float)ship.staticPressurekPa); _max_thrust += thrust; _cur_thrust += MFX.finalThrust; //Overheat stuff //if (!MFX.flameout) //{ // double temp = P.temperature / P.maxTemp; // if (temp > _max_temp) _max_temp = temp; //} if (1 / MFX.engineAccelerationSpeed > _engineaccel) { _engineaccel = 1 / MFX.engineAccelerationSpeed; } if (1 / MFX.engineDecelerationSpeed > _enginedecel) { _enginedecel = 1 / MFX.engineDecelerationSpeed; } foreach (Propellant Pro in MFX.propellants) { if (Pro.name.Equals("IntakeAir")) { _airreq += Pro.currentRequirement; } } } else if (pm is ModuleResourceIntake) { ModuleResourceIntake MRI = pm as ModuleResourceIntake; if (MRI.intakeEnabled && MRI.resourceName.Equals("IntakeAir")) { _airin += MRI.airFlow * dt; //Using dt courtesy of FAR } } } } _isp = _max_thrust / _isp; //Complete the average isp if (_engineaccel == 0) { _engineaccel = 2; //Default in case the engines don't define it } //Debug.Log("Engine Acceleration: " + Math.Round(_engineaccel, 2)); //Electric charge rate if (now - lastTime > 0.1) { _chargeRate = (_electricCharge - lastCharge) / (now - lastTime); lastCharge = _electricCharge; lastTime = now; } //Clamp to +30/-30 if (_chargeRate > 30) { _chargeRate = 30; } if (_chargeRate < -30) { _chargeRate = -30; } //Maneuver node burn calculations //dv = Isp * ln (m0 / m1) //e^(dv/ISP) = m0/m1 //m1 = m0/e^(dv/ISP) //mass flow = thrust/isp try { ManeuverNode myNode = FlightGlobals.ActiveVessel.patchedConicSolver.maneuverNodes.ToArray()[0]; double deltaVRem = myNode.GetBurnVector(ship.orbit).magnitude; //Remaining ΔV in the burn, per maneuver node //Debug.Log("dV: " + Math.Round(deltaVRem, 1)); //Debug.Log("Mass: " + Math.Round(_mass, 1) + " ISP: " + Math.Round(_isp, 1)); _final_mass = _mass / Math.Pow(Math.E, (deltaVRem / (_isp * 9.82))); //Mass after burn Changed from 9.821 //Debug.Log("Final Mass: " + Math.Round(_final_mass, 1)); _mass_rate = _max_thrust / (_isp * 9.82); //Mass flow rate //Debug.Log("Mass Rate: " + Math.Round(_mass_rate, 3)); //Debug.Log("Burn Mass: " + Math.Round(_mass - _final_mass, 1)); _burn_time = (_mass - _final_mass);// / _mass_rate; //Time it takes for this burn //Debug.Log("Burn Mass: " + _burn_time); _burn_time = _burn_time / _mass_rate; //Debug.Log("Burn Time: " + _burn_time); _burn_time += 2d; //_burn_time += _engineaccel + _enginedecel; //Factor in slow throttles } catch { _burn_time = 0; _final_mass = 0; _mass_rate = 0; } //Heading, courtesy of MechJeb /*Vector3d CoM, up, north, east, forward, rootPartPos; * Quaternion rotationSurface, rotationVesselSurface; * CoM = ship.findWorldCenterOfMass(); * up = (CoM - body.position).normalized; * Rigidbody rigidBody = ship.rootPart.rigidbody; * if (rigidBody != null) rootPartPos = rigidBody.position; * north = Vector3d.Exclude(up, (body.position + body.transform.up * (float)body.Radius) - CoM).normalized; * east = body.getRFrmVel(CoM).normalized; * forward = ship.GetTransform().up; * rotationSurface = Quaternion.LookRotation(north, up); * rotationVesselSurface = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(ship.GetTransform().rotation) * rotationSurface); * _heading = rotationVesselSurface.eulerAngles.y; */ _heading = FlightGlobals.ship_heading; //Debug.Log("FlightGlobals Heading: " + FlightGlobals.ship_heading + " MechJeb Heading: " + _heading); //good //Debug.Log("Ship temp: " + FlightGlobals.ship_temp); //not useful //Debug.Log("Get dV: " + FlightGlobals.ActiveVessel.GetDeltaV()); //nothing //Debug.Log("Surface Height: " + FlightGlobals.ActiveVessel.GetHeightFromSurface() + " Terrain Height: "+FlightGlobals.ActiveVessel.GetHeightFromTerrain()); //not useful //Debug.Log("GetMass(): " + FlightGlobals.ActiveVessel.GetTotalMass()+" myMass: "+_mass); //good //Debug.Log("IAS: " + FlightGlobals.ActiveVessel.indicatedAirSpeed + " M: " + FlightGlobals.ActiveVessel.mach); //good //Waypoint distance code //Uses the Spherical Law of Cosines from http://www.movable-type.co.uk/scripts/latlong.html NavWaypoint nW = NavWaypoint.fetch; //The active nav waypoint FinePrint.WaypointManager WM; WM = FinePrint.WaypointManager.Instance(); if (nW != null && nW.IsActive) { //Convert these all to radians to do actual math on them. double Aa = ship.latitude * Math.PI / 180; double Ao = ship.longitude * Math.PI / 180; double Ba = nW.Latitude * Math.PI / 180; double Bo = nW.Longitude * Math.PI / 180; _nav_dist = Math.Acos(Math.Sin(Aa) * Math.Sin(Ba) + Math.Cos(Aa) * Math.Cos(Ba) * Math.Cos(Bo - Ao)) * FlightGlobals.currentMainBody.Radius; //used to be 600000 double y = Math.Sin(Bo - Ao) * Math.Cos(Ba); double x = Math.Cos(Aa) * Math.Sin(Ba) - Math.Sin(Aa) * Math.Cos(Ba) * Math.Cos(Bo - Ao); double b = Math.Atan2(y, x) * 180 / Math.PI; //Converted to degrees. _nav_heading = (b + 360f) % 360f; //Convert to 0-360 from -180 to 180 } else if (_nav_waypoint != 0) { //defaults _nav_heading = -1; _nav_dist = -1; //now try actually calculating things int count = 0; foreach (FinePrint.Waypoint W in WM.Waypoints) { //we clearly only care about waypoints on this planet if (W.celestialName.Equals(body.name)) { count++; if (count == _nav_waypoint) { //Convert these all to radians to do actual math on them. double Aa = ship.latitude * Math.PI / 180; double Ao = ship.longitude * Math.PI / 180; double Ba = W.latitude * Math.PI / 180; double Bo = W.longitude * Math.PI / 180; _nav_dist = Math.Acos(Math.Sin(Aa) * Math.Sin(Ba) + Math.Cos(Aa) * Math.Cos(Ba) * Math.Cos(Bo - Ao)) * FlightGlobals.currentMainBody.Radius; double y = Math.Sin(Bo - Ao) * Math.Cos(Ba); double x = Math.Cos(Aa) * Math.Sin(Ba) - Math.Sin(Aa) * Math.Cos(Ba) * Math.Cos(Bo - Ao); double b = Math.Atan2(y, x) * 180 / Math.PI; //Converted to degrees. _nav_heading = (b + 360f) % 360f; //Convert to 0-360 from -180 to 180 } } } } else { _nav_dist = -1; //Both of these at -1 implies no nav data _nav_heading = -1f; } //Pitch, yaw, roll /*NavBall ball = FlightUIController.fetch.GetComponentInChildren<NavBall>(); * Quaternion vesselRot = Quaternion.Inverse(ball.relativeGymbal); * pitch = (vesselRot.eulerAngles.x > 180) ? (360 - vesselRot.eulerAngles.x) : -vesselRot.eulerAngles.x; * roll = (vesselRot.eulerAngles.z > 180) ? (360 - vesselRot.eulerAngles.z) : -vesselRot.eulerAngles.z; * yaw = vesselRot.eulerAngles.y; //Heading, more or less * _rel_yaw = AngleAroundNormal(ship.GetObtVelocity(), ship.ReferenceTransform.up, ship.ReferenceTransform.forward); * _rel_pitch = AngleAroundNormal(ship.GetObtVelocity(), ship.ReferenceTransform.up, ship.ReferenceTransform.right); * * //Polars * if (pitch > 0f) * { * Quaternion polarRot = Quaternion.Euler(90, 0, 0) * vesselRot; * polar_yaw = (polarRot.eulerAngles.y + 180f) % 360f - 180f; * polar_pitch = (polarRot.eulerAngles.x + 180f) % 360f - 180f; * polar_roll = polarRot.eulerAngles.z; * } * else * { * Quaternion polarRot = Quaternion.Euler(-90, 0, 0) * vesselRot; * polar_yaw = (polarRot.eulerAngles.y + 180f) % 360f - 180f; * polar_pitch = (polarRot.eulerAngles.x + 180f) % 360f - 180f; * polar_roll = polarRot.eulerAngles.z; * }*/ //Airspeed stuff getAirspeedInfo(ship, out _mach, out _tas, out _eas); //Max Altitude if (ship.Landed) { max_altitude = ship.altitude; //Reset max altitude upon landing/takeoff } if (ship.altitude > max_altitude) { max_altitude = ship.altitude; } //RA double terrain = ship.terrainAltitude; if (terrain < 0) { terrain = 0; } _ra = (long)(body.GetAltitude(ship.CoM) - terrain); //In orbit? inOrbit = false; //Minimum periapsis of 5km if (ship.orbit.PeA > 5000) { //Make sure our orbit is out of the atmosphere, if there is one. if (body.atmosphere) { if (ship.orbit.PeA > body.atmosphereDepth) //is this the same as max altitude? { inOrbit = true; } } else { inOrbit = true; } } //Closest approach and time, target distance and speed if (FlightGlobals.fetch.VesselTarget != null) { Orbit TO = FlightGlobals.fetch.VesselTarget.GetOrbit(); Vector3d aPos = ship.ReferenceTransform.position; //Control source's position Vector3d tPos = _tarpos = FlightGlobals.fetch.VesselTarget.GetTransform().position; //Rough distance if (FlightGlobals.fetch.VesselTarget is ModuleDockingNode) //Use more precise distance { ModuleDockingNode targetDockingPort = FlightGlobals.fetch.VesselTarget as ModuleDockingNode; tPos = _tarpos = targetDockingPort.controlTransform.position; } // MechJeb guidance targets _don't have an orbit_! else if (TO != null) { // This is in the wrong coordinate system for _tarpos, and only usable for distance aPos = ship.GetOrbit().pos; tPos = TO.pos; } _target_dist = Vector3d.Distance(aPos, tPos); } else { _target_dist = 0; } var target = FlightGlobals.fetch.VesselTarget; // MechJeb guidance targets _don't have an orbit! // Also, landed targets often have invalid orbit that causes error spam and lag. if (target != null && target.GetOrbit() != null) { Orbit O = ship.GetOrbit(); Orbit TO = target.GetOrbit(); double t = Planetarium.GetUniversalTime(); Func <double, Vector3d> targetPosAt = (d => TO.getPositionAtUT(t + d)); // For landed targets the orbit doesn't make sense, so calculate planet rotation if (target.GetVessel() != null && target.GetVessel().LandedOrSplashed) { Vessel ves = target.GetVessel(); CelestialBody vbody = ves.mainBody; Vector3d pos = vbody.GetRelSurfacePosition(ves.latitude, ves.longitude, ves.altitude); targetPosAt = delegate(double d) { double angle = vbody.rotates ? d * 360.0 / vbody.rotationPeriod : 0; return(vbody.position + QuaternionD.AngleAxis(angle, Vector3d.down) * pos); }; } //I chunck my orbit into 100 pieces, and find between which two chuncks the minimum occurs... double period = O.period; double bestDist = double.MaxValue; double bestTime = 0; for (double d = 0; d < period; d += period / 100) //Look at 100 places around the orbit { if (d == 0) { continue; //skip the first time } double dist = Vector3d.Distance(O.getPositionAtUT(t + d), targetPosAt(d)); if (dist < bestDist) { bestDist = dist; bestTime = d; } } //Now, do it again, but over a small section of the orbit centered on the above double start = bestTime - (period / 100); double end = bestTime + (period / 100); for (double d = start; d < end; d += period / 1000) //Look at 100 places within this chunck of orbit { if (d == start) { continue; //Skip the first time } double dist = Vector3d.Distance(O.getPositionAtUT(t + d), targetPosAt(d)); if (dist < bestDist) { bestDist = dist; bestTime = d; } } //And one last time, which is probably overkill start = bestTime - (period / 1000); end = bestTime + (period / 1000); for (double d = start; d < end; d += period / 10000) //Look at 100 places within this chunck of orbit { if (d == start) { continue; //For ease of computation } double dist = Vector3d.Distance(O.getPositionAtUT(t + d), targetPosAt(d)); if (dist < bestDist) { bestDist = dist; bestTime = d; //previous time is my start time } } _closest_time = bestTime; _closest_approach = bestDist; } else { _closest_approach = 0; _closest_time = 0; } //Time to Impact calculations double vertspeed = FlightGlobals.ActiveVessel.verticalSpeed; if (!ship.Landed && vertspeed < 0 && ship.orbit.PeA <= 0 && !ship.mainBody.atmosphere) { double Vf = Math.Sqrt((vertspeed * vertspeed) + 2 * _ra * _gravity); //t = 2d/(Vi+Vf) _tti = (Math.Abs(Vf) - Math.Abs(vertspeed)) / _gravity; _tti++; //So you hit the ground at 0, not 1 second after 0 } else { _tti = 0; } //Suicide Altitude calculations if (ship.Landed || ship.orbit.PeA > 0 || body.atmosphere) { _sa = -1; //Not calculating this } else { _sa = 0; double avgG = body.gravParameter / ((body.Radius + ship.terrainAltitude) * (body.Radius + ship.terrainAltitude)); avgG += FlightGlobals.getGeeForceAtPosition(ship.CoM).magnitude; avgG /= 2; //Debug.Log("Average G: " + Math.Round(avgG, 2)); double vdv = Math.Sqrt((2 * avgG * _ra) + (ship.verticalSpeed * ship.verticalSpeed)); //Debug.Log("Vertical Delta V: " + Math.Round(vdv, 1)); //Altitude Fraction = (Vertical dv ^2) / (2 * 1000 * Thrust (kN)) double altFrac = (vdv * vdv) / (2 * 1000 * _max_thrust); //Debug.Log("Altitude Fraction: " + Math.Round(altFrac, 2)); //m-avg = (m0 + (m0 / e ^ (dv / (Isp * 9.82)))) / 2 double avgMass = (_mass / Math.Pow(Math.E, (vdv / (_isp * 9.82)))); avgMass = (_mass + avgMass) / 2; //Debug.Log("Average mass: " + Math.Round(avgMass, 1)); _sa = (long)Math.Round(altFrac * avgMass * 1000); //Debug.Log("Suicide Altitude: " + Math.Round(_sa)); } //Cleanup isReadable = true; //Debug.Log("Thread simulation complete at " + Math.Round(Time.time, 3)); }
void WindowFunction(int windowID) { BoosterGuidanceCore core = CheckCore(FlightGlobals.ActiveVessel); if (core == null) { GUILayout.BeginHorizontal(); GUILayout.Label("No BoosterGuidance Core"); GUILayout.EndHorizontal(); return; } OnUpdate(); SetEnabledColors(true); // Close button if (GUI.Button(new Rect(windowRect.width - 18, 2, 16, 16), "")) { Hide(); return; } // Check for target being set if (core.vessel.targetObject != lastVesselTarget) { if (core.vessel.targetObject != null) { Vessel target = core.vessel.targetObject.GetVessel(); tgtLatitude = target.latitude; tgtLongitude = target.longitude; tgtAlt = (int)target.altitude; UpdateCore(); string msg = String.Format(Localizer.Format("#BoosterGuidance_TargetSetToX"), target.name); GuiUtils.ScreenMessage(msg); } lastVesselTarget = core.vessel.targetObject; } // Check for navigation target NavWaypoint nav = NavWaypoint.fetch; if (nav.IsActive) { // Does current nav position differ from last one used? A hack because // a can't see a way to check if the nav waypoint has changed // Doing it this way means lat and lon in window can be edited without them // getting locked to the nav waypoint if ((lastNavLat != nav.Latitude) || (lastNavLon != nav.Longitude)) { Coordinates pos = new Coordinates(nav.Latitude, nav.Longitude); Debug.Log("[BoosterGuidance] Target set to nav location " + pos.ToStringDMS()); tgtLatitude = nav.Latitude; tgtLongitude = nav.Longitude; lastNavLat = nav.Latitude; lastNavLon = nav.Longitude; // This is VERY unreliable //tgtAlt = (int)nav.Altitude; tgtAlt = (int)FlightGlobals.ActiveVessel.mainBody.TerrainAltitude(tgtLatitude, tgtLongitude); core.SetTarget(tgtLatitude, tgtLongitude, tgtAlt); string msg = String.Format(Localizer.Format("#BoosterGuidance_TargetSetToX"), pos.ToStringDMS()); GuiUtils.ScreenMessage(msg); UpdateCore(); } } else { lastNavLat = 0; lastNavLon = 0; } // Check for unloaded vessels foreach (var controller in BoosterGuidanceCore.controllers) { if (!controller.vessel.loaded) { GuiUtils.ScreenMessage("Guidance disabled for " + controller.vessel.name + " as out of physics range"); DisableGuidance(); } } tab = GUILayout.Toolbar(tab, new string[] { Localizer.Format("#BoosterGuidance_Main"), Localizer.Format("Advanced") }); bool changed = false; switch (tab) { case 0: changed = MainTab(windowID); break; case 1: changed = AdvancedTab(windowID); break; } if (changed) { UpdateCore(); } }
public static void updateNavigationData() { //see if information is current if (GetLastNavUpdateUT() != Planetarium.GetUniversalTime()) { if (currentBody == null || FlightGlobals.currentMainBody != currentBody) { rwyIdx = 0; currentBody = FlightGlobals.currentMainBody; currentBodyRunways.Clear(); for (int i = 0; i < allRunways.Count; i++) { if (allRunways[i].body == currentBody.name) { currentBodyRunways.Add(allRunways[i]); } } } selectedGlideSlope = gsList[gsIdx]; if (currentBodyRunways.Count == 0) { selectedRwy = null; rwyIdx = 0; fallback = true; } else { selectedRwy = currentBodyRunways[rwyIdx]; fallback = false; } //Since there seems to be no callback methods to determine whether waypoint has been set or changed, we have to refresh INS data on every update NavWaypoint navWaypoint = NavWaypoint.fetch; if ((navWaypoint != null) && navWaypoint.IsActive && navWaypoint.Body == FlightGlobals.currentMainBody) { Waypoint waypoint = null; if (prevWaypoint != null && navWaypoint.IsUsing(prevWaypoint)) { waypoint = prevWaypoint; } else { foreach (Waypoint wp in FinePrint.WaypointManager.Instance().Waypoints) { if (navWaypoint.IsUsing(wp)) { waypoint = wp; break; } } prevWaypoint = waypoint; } //If waypoint is fine then generate fake target runway every time Runway insTarget = new Runway { isINSTarget = true, ident = waypoint != null ? waypoint.name : navWaypoint.name, body = navWaypoint.Body.name, hdg = selectedRwy != null ? selectedRwy.hdg : 0, altMSL = (float)(navWaypoint.Height + navWaypoint.Altitude), locLatitude = (float)navWaypoint.Latitude, locLongitude = (float)navWaypoint.Longitude, gsLatitude = (float)navWaypoint.Latitude, gsLongitude = (float)navWaypoint.Longitude }; selectedRwy = insTarget; } currentVessel = FlightGlobals.ActiveVessel; if (selectedRwy != null) { bearing = NavUtilLib.Utils.CalcBearingToBeacon(currentVessel, selectedRwy); dme = NavUtilLib.Utils.CalcDistanceToBeacon(currentVessel, selectedRwy); elevationAngle = NavUtilLib.Utils.CalcElevationAngle(currentVessel, selectedRwy); //locDeviation = NavUtilLib.Utils.CalcLocalizerDeviation(bearing, selectedRwy); locDeviation = (float)NavUtilLib.Utils.CalcLocalizerDeviation(currentVessel, selectedRwy); gsDeviation = NavUtilLib.Utils.CalcGlideslopeDeviation(elevationAngle, selectedGlideSlope); // runwayHeading = (float)NavUtilLib.Utils.CalcProjectedRunwayHeading(currentVessel, selectedRwy); } else { bearing = 0; dme = 0; elevationAngle = 0; locDeviation = 0; gsDeviation = 0; runwayHeading = 0; selectedRwy = Runway.fallback(); } SetLastNavUpdateUT(); } }