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