protected void Init(Part p, ReentrySimulation.SimCurves _simCurves)
{
Rigidbody rigidbody = p.rb;
totalMass = rigidbody == null ? 0 : rigidbody.mass; // TODO : check if we need to use this or the one without the childMass
shieldedFromAirstream = p.ShieldedFromAirstream;
noDrag = rigidbody == null && !PhysicsGlobals.ApplyDragToNonPhysicsParts;
hasLiftModule = p.hasLiftModule;
bodyLiftMultiplier = p.bodyLiftMultiplier * PhysicsGlobals.BodyLiftMultiplier;
simCurves = _simCurves;
//cubes = new DragCubeList();
CopyDragCubesList(p.DragCubes, cubes);
// Rotation to convert the vessel space vesselVelocity to the part space vesselVelocity
// QuaternionD.LookRotation is not working...
partToVessel = Quaternion.LookRotation(p.vessel.GetTransform().InverseTransformDirection(p.transform.forward), p.vessel.GetTransform().InverseTransformDirection(p.transform.up));
vesselToPart = Quaternion.Inverse(partToVessel);
//DragCubeMultiplier = PhysicsGlobals.DragCubeMultiplier;
//DragMultiplier = PhysicsGlobals.DragMultiplier;
#if DEBUG
if (p.dragModel != Part.DragModel.CUBE)
{
Log.dbg("{0} {1}", p.name, p.dragModel);
}
#endif
//oPart = p;
}
void DriveAscent(FlightCtrlState s)
{
if (timedLaunch)
{
status = Localizer.Format("#MechJeb_Ascent_status6");//"Awaiting liftoff"
Log.dbg("Awaiting Liftoff");
// kill the optimizer if it is running.
core.guidance.enabled = false;
core.attitude.AxisControl(false, false, false);
return;
}
DriveDeployableComponents(s);
if (ascentPath.DriveAscent(s))
{
Log.detail("Remaining in Ascent");
status = ascentPath.status;
}
else
{
Log.detail("Ascend -> Circularize");
mode = AscentMode.CIRCULARIZE;
}
}
private void Init(ModuleParachute mp, double startTime, int limitChutesStage)
{
this.para = mp;
this.state = mp.deploymentState;
willDeploy = limitChutesStage != -1 && para.part.inverseStage >= limitChutesStage;
// Work out when the chute was put into its current state based on the current drag as compared to the stowed, semi deployed and fully deployed drag
double timeSinceDeployment = 0;
switch (mp.deploymentState)
{
case ModuleParachute.deploymentStates.SEMIDEPLOYED:
if (mp.Anim.isPlaying)
{
timeSinceDeployment = mp.Anim[mp.semiDeployedAnimation].time;
}
else
{
timeSinceDeployment = 10000000;
}
break;
case ModuleParachute.deploymentStates.DEPLOYED:
if (mp.Anim.isPlaying)
{
timeSinceDeployment = mp.Anim[mp.fullyDeployedAnimation].time;
}
else
{
timeSinceDeployment = 10000000;
}
break;
case ModuleParachute.deploymentStates.STOWED:
case ModuleParachute.deploymentStates.ACTIVE:
// If the parachute is stowed then for some reason para.parachuteDrag does not reflect the stowed drag. set this up by hand.
timeSinceDeployment = 10000000;
break;
default:
// otherwise set the time since deployment to be a very large number to indcate that it has been in that state for a long time (although we do not know how long!
timeSinceDeployment = 10000000;
break;
}
this.openningTime = startTime - timeSinceDeployment;
#if false
Log.dbg("Parachute {0} parachuteDrag:{1} stowedDrag:{2} semiDeployedDrag:{3} fullyDeployedDrag:{4} part.maximum_drag:{5} part.minimum_drag:{6} semiDeploymentSpeed:{7} deploymentSpeed:{8} deploymentState:{9} timeSinceDeployment:{10}", para.name, this.parachuteDrag, para.stowedDrag, para.semiDeployedDrag, para.fullyDeployedDrag, para.part.maximum_drag, para.part.minimum_drag, para.semiDeploymentSpeed, para.deploymentSpeed, para.deploymentState, timeSinceDeployment);
// Keep that test code until they fix the bug in the new parachute module
if ((realDrag / parachuteDrag) > 1.01d || (realDrag / parachuteDrag) < 0.99d)
{
Log.dbg("Parachute {0} parachuteDrag:{1:0.000} RealDrag:{2:0.000} MinDrag:{3:0.000} MaxDrag:{4:0.000}", para.name, this.parachuteDrag, realDrag, para.part.minimum_drag, para.part.maximum_drag);
}
#endif
}
// for a single EngineModule, get thrust + isp + massFlowRate
public static void EngineValuesAtConditions(this ModuleEngines e, double throttle, double atmPressure, double atmDensity, double machNumber, out Vector3d thrust, out double isp, out double massFlowRate, bool cosLoss = true)
{
isp = e.ISPAtConditions(throttle, atmPressure, atmDensity, machNumber);
double flowMultiplier = e.FlowMultiplierAtConditions(atmDensity, machNumber);
massFlowRate = e.FlowRateAtConditions(throttle, flowMultiplier);
thrust = e.ThrustAtConditions(massFlowRate, isp, cosLoss);
Log.dbg("thrust = {0} isp = {1} massFlowRate = {2}", thrust, isp, massFlowRate);
}
public override void OnFixedUpdate()
{
if (markUT == 0)
{
Mark();
}
timeSinceMark = vesselState.time - markUT;
if (vessel.situation == Vessel.Situations.PRELAUNCH)
{
Mark(); //keep resetting stats until we launch
return;
}
if (vesselState.currentThrustAccel > 0)
{
gravityLosses += vesselState.deltaT * Vector3d.Dot(-vesselState.orbitalVelocity.normalized, vesselState.gravityForce);
}
dragLosses += vesselState.deltaT * vesselState.drag;
deltaVExpended += vesselState.deltaT * vesselState.currentThrustAccel;
steeringLosses += vesselState.deltaT * vesselState.currentThrustAccel * (1 - Vector3d.Dot(vesselState.orbitalVelocity.normalized, vesselState.forward));
maxDragGees = Math.Max(maxDragGees, vesselState.drag / 9.81);
double circularPeriod = 2 * Math.PI * vesselState.radius / OrbitalManeuverCalculator.CircularOrbitSpeed(mainBody, vesselState.radius);
double angleTraversed = (vesselState.longitude - markLongitude) + 360 * (vesselState.time - markUT) / part.vessel.mainBody.rotationPeriod;
phaseAngleFromMark = MuUtils.ClampDegrees360(360 * (vesselState.time - markUT) / circularPeriod - angleTraversed);
if (paused)
{
return;
}
//int oldHistoryIdx = historyIdx;
//historyIdx = Mathf.Min(Mathf.FloorToInt((float)(timeSinceMark / precision)), history.Length - 1);
if (vesselState.time >= (lastRecordTime + precision) && historyIdx < history.Length - 1)
{
lastRecordTime = vesselState.time;
historyIdx++;
Record(historyIdx);
#if DEBUG
if (TimeWarp.WarpMode == TimeWarp.Modes.HIGH)
{
Log.dbg("WRP {0} {1:0} {2:0.00}", historyIdx, history[historyIdx].downRange, history[historyIdx].AoA);
}
else
{
Log.dbg("STD {0} {1:0} {2:0.00}", historyIdx, history[historyIdx].downRange, history[historyIdx].AoA);
}
#endif
}
}
//The KSP drag law is dv/dt = -b * v^2 where b is proportional to the air density and
//the ship's drag coefficient. In this equation b has units of inverse length. So 1/b
//is a characteristic length: a ship that travels this distance through air will lose a significant
//fraction of its initial velocity
public static double DragLength(this CelestialBody body, Vector3d pos, double dragCoeff, double mass)
{
double airDensity = FlightGlobals.getAtmDensity(FlightGlobals.getStaticPressure(pos, body), FlightGlobals.getExternalTemperature(pos, body));
if (airDensity <= 0)
{
return(Double.MaxValue);
}
Log.dbg("DragLength {0:0.00000} {1:0.00000}", airDensity, dragCoeff);
return(mass / (0.0005 * PhysicsGlobals.DragMultiplier * airDensity * dragCoeff));
}
public double MaxAllowedSpeed(Vector3d pos, Vector3d vel)
{
if (terrainRadius < pos.magnitude)
{
return(Double.MaxValue);
}
double vSpeed = Vector3d.Dot(vel, pos.normalized);
double ToF = (vSpeed + Math.Sqrt(vSpeed * vSpeed + 2 * g * (pos.magnitude - terrainRadius))) / g;
Log.dbg("ToF = {0:0.00}", ToF);
return(0.8 * (thrust - g) * ToF);
}
static MechJebModuleGuidanceController()
{
isLoadedPrincipia = ReflectionUtils.isAssemblyLoaded("ksp_plugin_adapter");
if (isLoadedPrincipia)
{
principiaEGNPCDOF = ReflectionUtils.getMethodByReflection("ksp_plugin_adapter", "principia.ksp_plugin_adapter.Interface", "ExternalGetNearestPlannedCoastDegreesOfFreedom", BindingFlags.NonPublic | BindingFlags.Static);
if (principiaEGNPCDOF == null)
{
Log.dbg("failed to find ExternalGetNearestPlannedCoastDegreesOfFreedom");
isLoadedPrincipia = false;
return;
}
}
}
private bool NeedToSave()
{
bool needToSave = false;
foreach (ComputerModule module in GetComputerModules <ComputerModule>())
{
#if DEBUG
if (module.dirty)
{
Log.dbg("{0} is dirty", module.profilerName);
}
#endif
needToSave |= module.dirty;
}
return(needToSave);
}
public override void OnUpdate()
{
if (!WarpPaused && lastAskedIndex > 0 && lastAskedIndex != TimeWarp.CurrentRateIndex)
{
// Rate limited by the altitude so we should not care
if (!vessel.LandedOrSplashed && TimeWarp.WarpMode == TimeWarp.Modes.HIGH && TimeWarp.CurrentRateIndex == TimeWarp.fetch.GetMaxRateForAltitude(vessel.altitude, vessel.mainBody))
{
return;
}
Log.dbg("Warppause : lastAskedIndex={0} CurrentRateIndex={1} WarpMode={2} MaxCurrentRate={3}", lastAskedIndex, TimeWarp.CurrentRateIndex, TimeWarp.WarpMode, TimeWarp.fetch.GetMaxRateForAltitude(vessel.altitude, vessel.mainBody));
WarpPaused = false;
//PauseWarp();
//ScreenMessages.PostScreenMessage("MJ : Warp canceled by user or an other mod");
}
}
// From FAR with ferram4 authorisation
public static MechJebModuleDockingAutopilot.Box3d GetBoundingBox(this Vessel vessel, bool debug = false)
{
Vector3 minBounds = new Vector3();
Vector3 maxBounds = new Vector3();
Log.dbg("[GetBoundingBox] Start {0}", vessel.vesselName);
for (int i = 0; i < vessel.parts.Count; i++)
{
Part p = vessel.parts[i];
PartExtensions.Vector3Pair partBox = p.GetBoundingBox();
Log.dbg("[GetBoundingBox] {0} {1:0.000}", p.name, (partBox.p1 - partBox.p2).magnitude);
maxBounds.x = Mathf.Max(maxBounds.x, partBox.p1.x);
minBounds.x = Mathf.Min(minBounds.x, partBox.p2.x);
maxBounds.y = Mathf.Max(maxBounds.y, partBox.p1.y);
minBounds.y = Mathf.Min(minBounds.y, partBox.p2.y);
maxBounds.z = Mathf.Max(maxBounds.z, partBox.p1.z);
minBounds.z = Mathf.Min(minBounds.z, partBox.p2.z);
//foreach (var sympart in p.symmetryCounterparts)
//{
// partBox = sympart.GetBoundingBox();
// maxBounds.x = Mathf.Max(maxBounds.x, partBox.p1.x);
// minBounds.x = Mathf.Min(minBounds.x, partBox.p2.x);
// maxBounds.y = Mathf.Max(maxBounds.y, partBox.p1.y);
// minBounds.y = Mathf.Min(minBounds.y, partBox.p2.y);
// maxBounds.z = Mathf.Max(maxBounds.z, partBox.p1.z);
// minBounds.z = Mathf.Min(minBounds.z, partBox.p2.z);
//}
}
if (debug)
{
Log.info("[GetBoundingBox] End {0}", vessel.vesselName);
}
MechJebModuleDockingAutopilot.Box3d box = new MechJebModuleDockingAutopilot.Box3d();
box.center = new Vector3d((maxBounds.x + minBounds.x) / 2, (maxBounds.y + minBounds.y) / 2, (maxBounds.z + minBounds.z) / 2);
box.size = new Vector3d(Math.Abs(box.center.x - maxBounds.x), Math.Abs(box.center.y - maxBounds.y), Math.Abs(box.center.z - maxBounds.z));
return(box);
}
protected void StartSimulation(bool addParachuteError)
{
double altitudeOfPreviousPrediction = 0;
double parachuteMultiplierForThisSimulation = this.parachuteSemiDeployMultiplier;
if (addParachuteError)
{
errorSimulationRunning = true;
errorStopwatch.Start(); //starts a timer that times how long the simulation takes
}
else
{
simulationRunning = true;
stopwatch.Start(); //starts a timer that times how long the simulation takes
}
Orbit patch = GetReenteringPatch() ?? orbit;
// Work out what the landing altitude was of the last prediction, and use that to pass into the next simulation
if (result != null)
{
if (result.outcome == ReentrySimulation.Outcome.LANDED && result.body != null)
{
altitudeOfPreviousPrediction = result.endASL; // Note that we are caling GetResult here to force the it to calculate the endASL, if it has not already done this. It is not allowed to do this previously as we are only allowed to do it from this thread, not the reentry simulation thread.
}
}
// Is this a simulation run with errors added? If so then add some error to the parachute multiple
if (addParachuteError)
{
parachuteMultiplierForThisSimulation *= 1d + (random.Next(1000000) - 500000d) / 10000000d;
}
// The curves used for the sim are not thread safe so we need a copy used only by the thread
ReentrySimulation.SimCurves simCurves = ReentrySimulation.SimCurves.Borrow(patch.referenceBody);
SimulatedVessel simVessel = SimulatedVessel.Borrow(vessel, simCurves, patch.StartUT, core.landing.enabled && deployChutes ? limitChutesStage : -1);
ReentrySimulation sim = ReentrySimulation.Borrow(patch, patch.StartUT, simVessel, simCurves, descentSpeedPolicy, decelEndAltitudeASL, vesselState.limitedMaxThrustAccel, parachuteMultiplierForThisSimulation, altitudeOfPreviousPrediction, addParachuteError, dt, Time.fixedDeltaTime, maxOrbits, noSkipToFreefall);
Log.dbg("Sim ran with dt={0:0.000}", dt);
//Run the simulation in a separate thread
ThreadPool.QueueUserWorkItem(RunSimulation, sim);
//RunSimulation(sim);
}
public void Reset()
{
// lambda and lambdaDot are deliberately not cleared here
Log.dbg("call stack: + {0}", Environment.StackTrace);
if (p != null)
{
p.KillThread();
p = null;
}
status = PVGStatus.INITIALIZING;
last_stage_time = 0.0;
last_optimizer_time = 0.0;
last_coasting_time = 0.0;
last_success_time = 0.0;
autowarp = false;
if (!MuUtils.PhysicsRunning())
{
core.warp.MinimumWarp();
}
}
public static void DebugLogList(List <double> l)
{
int i = 0;
string str = "";
for (int n1 = 0; n1 < l.Count; n1++)
{
str += String.Format("{0:F8}", l[n1]);
if (i % 6 == 5)
{
Log.dbg(str);
str = "";
}
else
{
str += " ";
}
i++;
}
}
// sma is only used for the initial guess but it is the responsibility of the caller
public void flightangle4constraintMAXE(double rTm, double gamma, double inc, double LAN, int numStages, double sma, bool omitCoast, bool currentInc)
{
if (status == PVGStatus.ENABLED)
{
return;
}
bool doupdate = false;
if (rTm != old_rTm || gamma != old_gamma || numStages != old_numStages || LAN != old_LAN)
{
doupdate = true;
}
// avoid slight drift in the current inclination from resetting guidance constantly
if (inc != old_inc && !currentInc)
{
doupdate = true;
}
if (p == null || doupdate)
{
if (p != null)
{
Log.dbg("killing a thread if its there to kill");
p.KillThread();
}
Log.info("MechJebModuleGuidanceController: setting up flightangle3constraintMAXE");
PontryaginLaunch solver = NewPontryaginForLaunch(inc, sma);
solver.omitCoast = omitCoast;
solver.flightangle4constraintMAXE(rTm, gamma * UtilMath.Deg2Rad, inc * UtilMath.Deg2Rad, LAN * UtilMath.Deg2Rad, numStages);
p = solver;
}
old_rTm = rTm;
old_gamma = gamma;
old_LAN = LAN;
old_numStages = numStages;
old_inc = inc;
}
void DrivePrelaunch(FlightCtrlState s)
{
if (vessel.LiftedOff() && !vessel.Landed)
{
status = Localizer.Format("#MechJeb_Ascent_status4");//"Vessel is not landed, skipping pre-launch"
mode = AscentMode.ASCEND;
return;
}
if (autoThrottle)
{
Log.info("prelaunch killing throttle");
core.thrust.ThrustOff();
}
core.attitude.AxisControl(false, false, false);
if (timedLaunch && tMinus > 10.0)
{
status = "Pre Launch";
return;
}
if (autodeploySolarPanels && mainBody.atmosphere)
{
core.solarpanel.RetractAll();
if (core.solarpanel.AllRetracted())
{
Log.dbg("Prelaunch -> Ascend");
mode = AscentMode.ASCEND;
}
else
{
status = Localizer.Format("#MechJeb_Ascent_status5");//"Retracting solar panels"
}
}
else
{
mode = AscentMode.ASCEND;
}
}
public double DecelerationEndAltitude()
{
if (UseAtmosphereToBrake())
{
// if the atmosphere is thick, deceleration (meaning freefall through the atmosphere)
// should end a safe height above the landing site in order to allow braking from terminal velocity
#warning Drag Length is quite large now without parachutes, check this better
double landingSiteDragLength = mainBody.DragLength(LandingAltitude, vesselAverageDrag + ParachuteAddedDragCoef(), vesselState.mass);
#if DEBUG
Log.dbg("DecelerationEndAltitude Atmo {0:00}", (2 * landingSiteDragLength + LandingAltitude));
#endif
return(1.1 * landingSiteDragLength + LandingAltitude);
}
else
{
//if the atmosphere is thin, the deceleration burn should end
//500 meters above the landing site to allow for a controlled final descent
double d = 500 + LandingAltitude;
Log.dbg("DecelerationEndAltitude Vacum ", d);
return(d);
}
}
void DeployParachutes()
{
if (vesselState.mainBody.atmosphere && deployChutes)
{
for (int i = 0; i < vesselState.parachutes.Count; i++)
{
ModuleParachute p = vesselState.parachutes[i];
// what is the ASL at which we should deploy this parachute? It is the actual deployment height above the surface + the ASL of the predicted landing point.
double LandingSiteASL = LandingAltitude;
double ParachuteDeployAboveGroundAtLandingSite = p.deployAltitude * this.parachutePlan.Multiplier;
double ASLDeployAltitude = ParachuteDeployAboveGroundAtLandingSite + LandingSiteASL;
if (p.part.inverseStage >= limitChutesStage && p.deploymentState == ModuleParachute.deploymentStates.STOWED &&
ASLDeployAltitude > vesselState.altitudeASL && p.deploymentSafeState == ModuleParachute.deploymentSafeStates.SAFE)
{
p.Deploy();
Log.dbg("Deploying parachute {0} at {1}. ({2} + {3})", p.name, ASLDeployAltitude, LandingSiteASL, ParachuteDeployAboveGroundAtLandingSite);
}
}
}
}
public virtual Vector3d Lift(Vector3d vesselVelocity, double liftFactor)
{
if (shieldedFromAirstream || hasLiftModule)
{
return(Vector3d.zero);
}
// direction of the lift in a vessel centric reference
Vector3d liftV = partToVessel * ((Vector3d)cubes.LiftForce * bodyLiftMultiplier * liftFactor);
Vector3d liftVector = liftV.ProjectOnPlane(vesselVelocity);
#if DEBUG && false
if (vesselVelocity.sqrMagnitude > 1 && oPart.DragCubes.LiftForce.sqrMagnitude > 0.001)
{
string msg = oPart.name;
Vector3 bodyL = oPart.transform.rotation * (oPart.bodyLiftScalar * oPart.DragCubes.LiftForce);
Vector3 bodyLift = Vector3.ProjectOnPlane(bodyL, -oPart.dragVectorDir);
msg += "\n liftDir " + MuUtils.PrettyPrint(liftVector) + " vs " + MuUtils.PrettyPrint(bodyLift) + " " + Vector3.Angle(liftVector, bodyLift).ToString("F3") + "°";
Vector3 localBodyL = oPart.vessel.transform.InverseTransformDirection(bodyL);
msg += "\n liftV " + MuUtils.PrettyPrint(liftV) + " vs " + MuUtils.PrettyPrint(localBodyL) + " " + Vector3.Angle(liftV, localBodyL).ToString("F3") + "°";
msg += "\n liftForce " + MuUtils.PrettyPrint(cubes.LiftForce) + " vs " + MuUtils.PrettyPrint(oPart.DragCubes.LiftForce) + " " + Vector3.Angle(cubes.LiftForce, oPart.DragCubes.LiftForce).ToString("F3") + "°";
msg += "\n Normals " + MuUtils.PrettyPrint(-vesselVelocity) + " vs " + MuUtils.PrettyPrint(-oPart.dragVectorDir) + " " + Vector3.Angle(-vesselVelocity, -oPart.dragVectorDir).ToString("F3") + "°";
msg += "\n vals " + bodyLiftMultiplier.ToString("F5") + " " + dynamicPressurekPa.ToString("F5") + " " + liftCurves.liftMachCurve.Evaluate(mach).ToString("F5");
Log.dbg(msg);
}
#endif
return(liftVector);
}
public override void OnStart(PartModule.StartState state)
{
Log.dbg("ModExtensionTest adding MJ2 callback");
//vesselState.vesselStatePartExtensions.Add(partUpdate);
//vesselState.vesselStatePartModuleExtensions.Add(partModuleUpdate);
}
private void converge()
{
if (p == null)
{
status = PVGStatus.INITIALIZING;
return;
}
if (p.last_success_time > 0)
{
last_success_time = p.last_success_time;
}
if (p.solution == null)
{
/* we have a solver but no solution */
status = PVGStatus.INITIALIZING;
}
else
{
/* we have a solver and have a valid solution */
if (isTerminalGuidance())
{
return;
}
/* hardcoded 10 seconds of terminal guidance */
if (tgo < 10)
{
// drop out of warp for terminal guidance (smaller time ticks => more accuracy)
core.warp.MinimumWarp();
status = PVGStatus.TERMINAL;
return;
}
}
if ((vesselState.time - last_optimizer_time) < MuUtils.Clamp(pvgInterval, 1.00, 30.00))
{
return;
}
// for last 10 seconds of coast phase don't recompute (FIXME: can this go lower? it was a workaround for a bug)
if (p.solution != null && p.solution.arc(vesselState.time).thrust == 0 && p.solution.current_tgo(vesselState.time) < 10)
{
return;
}
p.threadStart(vesselState.time);
#if DEBUG
if (p.threadStart(vesselState.time))
{
Log.dbg("MechJeb: started optimizer thread");
}
#endif
if (status == PVGStatus.INITIALIZING && p.solution != null)
{
status = PVGStatus.CONVERGED;
}
last_optimizer_time = vesselState.time;
}
private DifferentialThrottleStatus ComputeDifferentialThrottle(Vector3d torque)
{
#if DEBUG
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
#endif
float mainThrottle = vessel.ctrlState.mainThrottle;
if (mainThrottle == 0)
{
torque = Vector3d.zero;
mainThrottle = 1;
}
int nb_engines = vesselState.enginesWrappers.Count;
double torqueScale = 0;
double forceScale = 0;
Vector3d force = new Vector3d();
for (int i = 0; i < nb_engines; i++)
{
torque -= vesselState.enginesWrappers[i].constantTorque;
torqueScale += vesselState.enginesWrappers[i].maxVariableTorque.magnitude;
force += Vector3d.Dot(mainThrottle * vesselState.enginesWrappers[i].maxVariableForce, Vector3d.up) * Vector3d.up;
forceScale += vesselState.enginesWrappers[i].maxVariableForce.magnitude * 10;
}
var engines = vesselState.enginesWrappers.Where(eng => !eng.engine.throttleLocked).ToList();
var n = engines.Count;
if (nb_engines == 0)
{
return(DifferentialThrottleStatus.AllEnginesOff);
}
if (nb_engines == 1 || n == 0)
{
return(DifferentialThrottleStatus.MoreEnginesRequired);
}
double[,] a = new double[n, n];
double[] b = new double[n];
double[] boundL = new double[n];
double[] boundU = new double[n];
for (int i = 0; i < n; i++)
{
for (int j = 0; j < n; j++)
{
a[i, j] = Vector3d.Dot(engines[i].maxVariableTorque, engines[j].maxVariableTorque) / (torqueScale * torqueScale)
+ Vector3d.Dot(engines[i].maxVariableForce, engines[j].maxVariableForce) / (forceScale * forceScale);
}
b[i] = -Vector3d.Dot(engines[i].maxVariableTorque, torque) / (torqueScale * torqueScale)
- Vector3d.Dot(engines[i].maxVariableForce, force) / (forceScale * forceScale);
boundL[i] = 0;
boundU[i] = mainThrottle;
}
alglib.minqpstate state;
alglib.minqpcreate(n, out state);
alglib.minqpsetquadraticterm(state, a);
alglib.minqpsetlinearterm(state, b);
alglib.minqpsetbc(state, boundL, boundU);
alglib.minqpsetalgobleic(state, 0.0, 0.0, 0.0, 0);
#if DEBUG
var t1 = stopwatch.ElapsedMilliseconds;
#endif
alglib.minqpoptimize(state);
#if DEBUG
var t2 = stopwatch.ElapsedMilliseconds;
#endif
double[] x;
alglib.minqpreport report;
alglib.minqpresults(state, out x, out report);
#if DEBUG
var t3 = stopwatch.ElapsedMilliseconds;
Log.dbg("[DiffThrottle] t1: {0}, t2: {1}, t3: {2}", t1, t2 - t1, t3 - t2);
#endif
if (x.Any(val => double.IsNaN(val)))
{
return(DifferentialThrottleStatus.SolverFailed);
}
for (int i = 0; i < n; i++)
{
engines[i].thrustRatio = (float)(x[i] / mainThrottle);
}
return(DifferentialThrottleStatus.Success);
}
private void RunSimulation(object o)
{
ReentrySimulation sim = (ReentrySimulation)o;
try
{
ReentrySimulation.Result newResult = sim.RunSimulation();
lock (readyResults)
{
readyResults.Enqueue(newResult);
}
if (newResult.multiplierHasError)
{
//see how long the simulation took
errorStopwatch.Stop();
long millisecondsToCompletion = errorStopwatch.ElapsedMilliseconds;
lastErrorSimTime = millisecondsToCompletion * 0.001;
lastErrorSimSteps = newResult.steps;
errorStopwatch.Reset();
//set the delay before the next simulation
millisecondsBetweenErrorSimulations = Math.Min(Math.Max(4 * millisecondsToCompletion, 400), 5);
// Note that we are going to run the simulations with error in less often that the real simulations
//start the stopwatch that will count off this delay
errorStopwatch.Start();
errorSimulationRunning = false;
}
else
{
//see how long the simulation took
stopwatch.Stop();
long millisecondsToCompletion = stopwatch.ElapsedMilliseconds;
stopwatch.Reset();
//set the delay before the next simulation
millisecondsBetweenSimulations = Math.Min(Math.Max(2 * millisecondsToCompletion, 200), 5);
lastSimTime = millisecondsToCompletion * 0.001;
lastSimSteps = newResult.steps;
// Do not wait for too long before running another simulation, but also give the processor a rest.
// How long should we set the max_dt to be in the future? Calculate for interationsPerSecond runs per second. If we do not enter the atmosphere, however do not do so as we will complete so quickly, it is not a good guide to how long the reentry simulation takes.
if (newResult.outcome == ReentrySimulation.Outcome.AEROBRAKED ||
newResult.outcome == ReentrySimulation.Outcome.LANDED)
{
if (this.variabledt)
{
dt = newResult.maxdt * (millisecondsToCompletion / 1000d) / (1d / (3d * interationsPerSecond));
// There is no point in having a dt that is smaller than the physics frame rate as we would be trying to be more precise than the game.
dt = Math.Max(dt, sim.min_dt);
// Set a sensible upper limit to dt as well. - in this case 10 seconds
dt = Math.Min(dt, 10);
}
}
Log.dbg("Result: {0} Time to run: {1} millisecondsBetweenSimulations: {2} new dt: {3} Time.fixedDeltaTime {4}\n{5}", this.result.outcome, millisecondsToCompletion, millisecondsBetweenSimulations, dt, Time.fixedDeltaTime, this.result); // Note the endASL will be zero as it has not yet been calculated, and we are not allowed to calculate it from this thread :(
//start the stopwatch that will count off this delay
stopwatch.Start();
simulationRunning = false;
}
}
catch (Exception ex)
{
Log.aerr(ex, "Exception in MechJebModuleLandingPredictions.RunSimulation\n{0}", ex.StackTrace);
}
finally
{
sim.Release();
}
}
public override void OnLoad(ConfigNode sfsNode)
{
if (GuiUtils.skin == null)
{
//GuiUtils.skin = new GUISkin();
new GameObject("zombieGUILoader", typeof(ZombieGUILoader));
}
try
{
bool generateDefaultWindows = false;
base.OnLoad(sfsNode); //is this necessary?
// With the Unity 4.6 upgrade of KSP 1.0 we inherited a serialization problem
// with object with high depth like config nodes
// so the partmodule config node passed was not ok.
// So we use a static dir to save the part config node.
if (!savedConfig.ContainsKey(part.name))
{
if (HighLogic.LoadedScene == GameScenes.LOADING)
{
savedConfig.Add(part.name, sfsNode);
}
}
else
{
partSettings = savedConfig[part.name];
}
LoadComputerModules();
ConfigNode global = new ConfigNode("MechJebGlobalSettings");
if (File.Exists <MechJebCore>("mechjeb_settings_global.cfg"))
{
try
{
global = ConfigNode.Load(IOUtils.GetFilePathFor(this.GetType(), "mechjeb_settings_global.cfg"));
}
catch (Exception e)
{
Log.err(e, "MechJebCore.OnLoad caught an exception trying to load mechjeb_settings_global.cfg: {0}", e);
generateDefaultWindows = true;
}
}
else
{
generateDefaultWindows = true;
}
ConfigNode type = new ConfigNode("MechJebTypeSettings");
string vesselName = vessel != null?string.Join("_", vessel.vesselName.Split(System.IO.Path.GetInvalidFileNameChars())) : ""; // Strip illegal char from the filename
if ((vessel != null) && File.Exists <MechJebCore>("mechjeb_settings_type_" + vesselName + ".cfg"))
{
try
{
type = ConfigNode.Load(IOUtils.GetFilePathFor(this.GetType(), "mechjeb_settings_type_" + vesselName + ".cfg"));
}
catch (Exception e)
{
Log.err(e, "MechJebCore.OnLoad caught an exception trying to load mechjeb_settings_type_{0}.cfg: {1}", vesselName, e);
}
}
ConfigNode local = new ConfigNode("MechJebLocalSettings");
if (sfsNode != null && sfsNode.HasNode("MechJebLocalSettings"))
{
local = sfsNode.GetNode("MechJebLocalSettings");
}
else if (partSettings != null && partSettings.HasNode("MechJebLocalSettings"))
{
local = partSettings.GetNode("MechJebLocalSettings");
}
else if (sfsNode == null) // capture current Local settings
{
foreach (ComputerModule module in GetComputerModules <ComputerModule>())
{
try
{
module.OnSave(local.AddNode(module.GetType().Name), null, null);
}
catch (Exception e)
{
Log.err(e, "module {0} threw an exception in OnLoad: {1}", module.GetType().Name, e);
}
}
}
Log.dbg("OnLoad: loading from\n\tLocal: {0}\n\tType: {1}\n\tGlobal: {0}", local, type, global);
// Remove any currently loaded custom windows
MechJebModuleCustomInfoWindow win;
while ((win = GetComputerModule <MechJebModuleCustomInfoWindow>()) != null)
{
RemoveComputerModule(win);
}
foreach (ComputerModule module in GetComputerModules <ComputerModule>())
{
try
{
string name = module.GetType().Name;
ConfigNode moduleLocal = local.HasNode(name) ? local.GetNode(name) : null;
ConfigNode moduleType = type.HasNode(name) ? type.GetNode(name) : null;
ConfigNode moduleGlobal = global.HasNode(name) ? global.GetNode(name) : null;
module.OnLoad(moduleLocal, moduleType, moduleGlobal);
}
catch (Exception e)
{
Log.err("module threw an exception in OnLoad: {0} {1}", module.GetType().Name, e);
}
}
LoadDelayedModules();
if (generateDefaultWindows)
{
GetComputerModule <MechJebModuleCustomWindowEditor>().AddDefaultWindows();
}
}
catch (ReflectionTypeLoadException ex)
{
Log.err("caught a ReflectionTypeLoadException. Those DLL meeds maintenance and can cause serious collateral effects on the system if not fixed:");
var brokenAssembly = ex.Types.Where(x => x != null).Select(x => x.Assembly).Distinct();
foreach (Assembly assembly in brokenAssembly)
{
Log.err("{0} {1} {2}",
assembly.GetName().Name, assembly.GetName().Version, assembly.Location.Remove(0, Path.GetFullPath(KSPUtil.ApplicationRootPath).Length)
);
}
}
catch (Exception e)
{
Log.err(e, "caught exception in core OnLoad: {0}", e);
}
}
public void Update()
{
if (this != vessel.GetMasterMechJeb() || (!FlightGlobals.ready && HighLogic.LoadedSceneIsFlight) || !ready)
{
return;
}
Profiler.BeginSample("MechJebCore.Update");
if (Input.GetKeyDown(KeyCode.V) && (Input.GetKey(KeyCode.LeftControl) || Input.GetKey(KeyCode.RightControl)))
{
MechJebModuleCustomWindowEditor windowEditor = GetComputerModule <MechJebModuleCustomWindowEditor>();
if (windowEditor != null)
{
windowEditor.CreateWindowFromSharingString(MuUtils.SystemClipboard);
}
}
//periodically save settings in case we quit unexpectedly
Profiler.BeginSample("MechJebCore.Update.OnSave");
if (HighLogic.LoadedSceneIsEditor || (vessel != null && vessel.isActiveVessel))
{
if (Time.time > lastSettingsSaveTime + 5)
{
if (NeedToSave())
{
Log.dbg("Periodic settings save");
OnSave(null);
}
lastSettingsSaveTime = Time.time;
}
}
Profiler.EndSample();
if (ResearchAndDevelopment.Instance != null && unorderedComputerModules.Any(a => !a.unlockChecked))
{
foreach (ComputerModule module in GetComputerModules <ComputerModule>())
{
try
{
module.UnlockCheck();
}
catch (Exception e)
{
Log.err(e, "module {0} threw an exception in UnlockCheck: {1}", module.GetType().Name, e);
}
}
}
Profiler.BeginSample("OnMenuUpdate");
GetComputerModule <MechJebModuleMenu>().OnMenuUpdate(); // Allow the menu movement, even while in Editor
Profiler.EndSample();
if (vessel == null)
{
Profiler.EndSample();
return; //don't run ComputerModules' OnUpdate in editor
}
foreach (ComputerModule module in GetComputerModules <ComputerModule>())
{
Profiler.BeginSample(module.profilerName);
try
{
if (module.enabled)
{
module.OnUpdate();
}
}
catch (Exception e)
{
Log.err(e, "module {0} threw an exception in OnUpdate: {1}", module.GetType().Name, e);
}
Profiler.EndSample();
}
Profiler.EndSample();
}
public virtual Vector3d Drag(Vector3d vesselVelocity, double dragFactor, float mach)
{
if (shieldedFromAirstream || noDrag)
{
return(Vector3d.zero);
}
Vector3d dragVectorDirLocal = -(vesselToPart * vesselVelocity).normalized;
cubes.SetDrag(dragVectorDirLocal, mach);
Vector3d drag = -vesselVelocity.normalized * cubes.AreaDrag * dragFactor;
#if DEBUG && false
bool delta = false;
string msg = oPart.name;
if (vesselVelocity.sqrMagnitude > 1 && dynamicPressurekPa - oPart.dynamicPressurekPa > oPart.dynamicPressurekPa * 0.1)
{
msg += " dynamicPressurekPa " + dynamicPressurekPa.ToString("f4") + " vs " + oPart.dynamicPressurekPa.ToString("f4");
delta = true;
}
//if (vesselVelocity.sqrMagnitude > 1 && cubes.AreaDrag - oPart.DragCubes.AreaDrag > oPart.DragCubes.AreaDrag * 0.1)
if (vesselVelocity.sqrMagnitude > 1)
{
msg += "\n AreaDrag " + cubes.AreaDrag.ToString("f4") + " vs " + oPart.DragCubes.AreaDrag.ToString("f4");
//msg += "\n mach " + mach.ToString("f4") + " vs " + oPart.machNumber.ToString("f4");
msg += "\n dragDir " + MuUtils.PrettyPrint(dragDir) + " vs " + MuUtils.PrettyPrint(oPart.dragVectorDirLocal) + " " + Vector3.Angle(dragDir, oPart.dragVectorDirLocal).ToString("F3") + "°";
//msg += "\n dragVel " + MuUtils.PrettyPrint(vesselVelocity.normalized) + " vs " + MuUtils.PrettyPrint(oPart.dragVector.normalized) + " " + Vector3.Angle(vesselVelocity.normalized, oPart.dragVector).ToString("F3") + "°";
msg += "\n Real° " + MuUtils.PrettyPrint(oPart.dragVectorDirLocal) + " " + Vector3.Angle(oPart.dragVectorDirLocal, Vector3.down).ToString("F3") + "°";
msg += "\n Sim° " + MuUtils.PrettyPrint(dragDir) + " " + Vector3.Angle(dragDir, Vector3.down).ToString("F3") + "°";
msg += "\n toUp " + MuUtils.PrettyPrint(vesselToPart * Vector3.up) + Vector3.Angle(vesselToPart * Vector3.up, Vector3.up).ToString("F3") + "°";
Vector3 quatUp = vesselToPart * Vector3.up;
Vector3 shipUp = oPart.vessel.transform.InverseTransformDirection(oPart.transform.up);
msg += "\n Ups " + MuUtils.PrettyPrint(quatUp) + " vs " + MuUtils.PrettyPrint(shipUp) + " " + Vector3.Angle(quatUp, shipUp).ToString("F3") + "°";
//msg += "\n AreaOccluded ";
//for (int i = 0; i < 6; i++)
//{
// msg += cubes.AreaOccluded[i].ToString("F3") + "/" + oPart.DragCubes.AreaOccluded[i].ToString("F3") + " ";
//}
//msg += "\n WeightedDrag ";
//for (int i = 0; i < 6; i++)
//{
// msg += cubes.WeightedDrag[i].ToString("F3") + "/" + oPart.DragCubes.WeightedDrag[i].ToString("F3") + " ";
//}
msg += "\n vesselToPart " + MuUtils.PrettyPrint(vesselToPart.eulerAngles);
delta = true;
}
if (delta)
{
Log.dbg(msg);
}
#endif
return(drag);
}
public override void OnSave(ConfigNode sfsNode)
{
//we have nothing worth saving if we're outside the editor or flight scenes:
if (!(HighLogic.LoadedSceneIsEditor || HighLogic.LoadedSceneIsFlight))
{
return;
}
// Only Masters can save
if (this != vessel.GetMasterMechJeb())
{
return;
}
//KSP calls OnSave *before* OnLoad when the first command pod is created in the editor.
//Defend against saving empty settings.
if (unorderedComputerModules.Count == 0)
{
return;
}
// .23 added a call to OnSave for undocking/decoupling vessel before they are properly init ...
if (HighLogic.LoadedSceneIsFlight && vessel != null && vessel.vesselName == null)
{
return;
}
Profiler.BeginSample("MechJebCore.OnSave");
try
{
//Add any to-be-loaded modules so they get saved properly
Profiler.BeginSample("MechJebCore.OnSave.LoadDelayedModules");
LoadDelayedModules();
// base.OnSave(sfsNode); //is this necessary?
Profiler.EndSample();
Profiler.BeginSample("MechJebCore.OnSave.ConfigNode");
ConfigNode local = new ConfigNode("MechJebLocalSettings");
ConfigNode type = new ConfigNode("MechJebTypeSettings");
ConfigNode global = new ConfigNode("MechJebGlobalSettings");
Profiler.EndSample();
Profiler.BeginSample("MechJebCore.OnSave.loop");
foreach (ComputerModule module in GetComputerModules <ComputerModule>())
{
Profiler.BeginSample(module.profilerName);
try
{
string name = module.GetType().Name;
module.OnSave(local.AddNode(name), type.AddNode(name), global.AddNode(name));
}
catch (Exception e)
{
Log.err(e, "module {0} threw an exception in OnSave: {0} {1}", module.GetType().Name, e);
}
Profiler.EndSample();
}
Log.dbg("OnSave:\n\tLocal: {0}\n\tType: {1}\n\tGlobal: {2}", local, type, global);
Profiler.EndSample();
Profiler.BeginSample("MechJebCore.OnSave.sfsNode");
if (sfsNode != null)
{
sfsNode.nodes.Add(local);
}
Profiler.EndSample();
Profiler.BeginSample("MechJebCore.OnSave.vesselName");
// The EDITOR => FLIGHT transition is annoying to handle. OnDestroy is called when HighLogic.LoadedSceneIsEditor is already false
// So we don't save in that case, which is not that bad since nearly nothing use vessel settings in the editor.
if (vessel != null || (HighLogic.LoadedSceneIsEditor && EditorLogic.fetch != null))
{
string vesselName = (HighLogic.LoadedSceneIsEditor && EditorLogic.fetch ? EditorLogic.fetch.shipNameField.text : vessel.vesselName);
vesselName = string.Join("_", vesselName.Split(Path.GetInvalidFileNameChars())); // Strip illegal char from the filename
type.Save(IOUtils.GetFilePathFor(this.GetType(), "mechjeb_settings_type_" + vesselName + ".cfg"));
}
Profiler.EndSample();
Profiler.BeginSample("MechJebCore.OnSave.global");
if (lastFocus == vessel)
{
global.Save(IOUtils.GetFilePathFor(this.GetType(), "mechjeb_settings_global.cfg"));
}
Profiler.EndSample();
}
catch (Exception e)
{
Log.err(e, "MechJeb caught exception in core OnSave: {0}", e);
}
Profiler.EndSample();
}
// Incorporates a new simulation result into the simulation data set and calculate a new semi deployment multiplier. If the data set has a poor correlation, then it might just leave the mutiplier. If the correlation becomes positive then it will clear the dataset and start again.
public void AddResult(ReentrySimulation.Result newResult)
{
// if this result is the same as the old result, then it is not new!
if (newResult.multiplierHasError)
{
if (lastErrorResult != null)
{
if (newResult.id == lastErrorResult.id)
{
return;
}
}
lastErrorResult = newResult;
}
else
{
if (lastResult != null)
{
if (newResult.id == lastResult.id)
{
return;
}
}
lastResult = newResult;
}
// What was the overshoot for this new result?
double overshoot = newResult.GetOvershoot(this.autoPilot.core.target.targetLatitude, this.autoPilot.core.target.targetLongitude);
Log.dbg("overshoot: {0:0.00} multiplier: {1:0.0000} hasError:{2}", overshoot, newResult.parachuteMultiplier, newResult.multiplierHasError);
// Add the new result to the linear regression
regression.Add(overshoot, newResult.parachuteMultiplier);
// What is the correlation coefficent of the data. If it is weak a correlation then we will dismiss the dataset and use it to change the current multiplier
correlation = regression.CorrelationCoefficent;
if (correlation > -0.2) // TODO this is the best value to test for non-correlation?
{
// If the correlation is less that 0 then we will give up controlling the parachutes and throw away the dataset. Also check that we have got several bits of data, just in case we get two datapoints that are badly correlated.
if (correlation > 0 && this.regression.dataSetSize > 5)
{
ClearData();
Log.dbg("Giving up control of the parachutes as the data does not correlate: {0}", correlation);
}
else
{
Log.dbg("Ignoring the simulation dataset because the correlation is not significant enough: {0}", correlation);
}
}
else
{
// How much data is there? If just one datapoint then we need to slightly vary the multplier to avoid doing exactly the same multiplier again and getting a divide by zero!. If there is just two then we will not update the multiplier as we can't conclude much from two points of data!
int dataSetSize = regression.dataSetSize;
if (dataSetSize == 1)
{
this.currentMultiplier *= 0.99999;
}
else if (dataSetSize == 2)
{
// Doing nothing
}
else
{
// Use the linear regression to give us a new prediciton for when to open the parachutes
try
{
this.currentMultiplier = regression.yIntercept;
}
catch (Exception)
{
// If there is not enough data then we expect an exception. However we need to vary the multiplier everso slightly so that we get different data in order to start generating data. This should never happen as we have already checked the size of the dataset.
this.currentMultiplier *= 0.99999;
}
}
// Impose sensible limits on the multiplier
if (this.currentMultiplier < 1 || double.IsNaN(currentMultiplier))
{
this.currentMultiplier = 1;
}
if (this.currentMultiplier > this.maxMultiplier)
{
this.currentMultiplier = this.maxMultiplier;
}
}
return;
}
public override void Drive(FlightCtrlState s) // TODO put the brake in when running out of power to prevent nighttime solar failures on hills, or atleast try to
{ // TODO make distance calculation for 'reached' determination consider the rover and waypoint on sealevel to prevent height differences from messing it up -- should be done now?
if (orbit.referenceBody != lastBody)
{
WaypointIndex = -1; Waypoints.Clear();
}
MechJebWaypoint wp = (WaypointIndex > -1 && WaypointIndex < Waypoints.Count ? Waypoints[WaypointIndex] : null);
var brake = vessel.ActionGroups[KSPActionGroup.Brakes]; // keep brakes locked if they are
curSpeed = Vector3d.Dot(vesselState.surfaceVelocity, vesselState.forward);
CalculateTraction();
speedIntAcc = speedPID.intAccum;
if (wp != null && wp.Body == orbit.referenceBody)
{
if (ControlHeading)
{
heading.val = Math.Round(HeadingToPos(vessel.CoM, wp.Position), 1);
}
if (ControlSpeed)
{
var nextWP = (WaypointIndex < Waypoints.Count - 1 ? Waypoints[WaypointIndex + 1] : (LoopWaypoints ? Waypoints[0] : null));
var distance = Vector3.Distance(vessel.CoM, wp.Position);
if (wp.Target != null)
{
distance += (float)(wp.Target.srfSpeed * curSpeed) / 2;
}
// var maxSpeed = (wp.MaxSpeed > 0 ? Math.Min((float)speed, wp.MaxSpeed) : speed); // use waypoints maxSpeed if set and smaller than set the speed or just stick with the set speed
var maxSpeed = (wp.MaxSpeed > 0 ? wp.MaxSpeed : speed); // speed used to go towards the waypoint, using the waypoints maxSpeed if set or just stick with the set speed
var minSpeed = (wp.MinSpeed > 0 ? wp.MinSpeed :
(nextWP != null ? TurningSpeed((nextWP.MaxSpeed > 0 ? nextWP.MaxSpeed : speed), heading - HeadingToPos(wp.Position, nextWP.Position)) :
(distance - wp.Radius > 50 ? turnSpeed.val : 1)));
minSpeed = (wp.Quicksave ? 1 : minSpeed);
// ^ speed used to go through the waypoint, using half the set speed or maxSpeed as minSpeed for routing waypoints (all except the last)
var newSpeed = Math.Min(maxSpeed, Math.Max((distance - wp.Radius) / curSpeed, minSpeed)); // brake when getting closer
newSpeed = (newSpeed > turnSpeed ? TurningSpeed(newSpeed, headingErr) : newSpeed); // reduce speed when turning a lot
// if (LimitAcceleration) { newSpeed = curSpeed + Mathf.Clamp((float)(newSpeed - curSpeed), -1.5f, 0.5f); }
// newSpeed = tgtSpeed + Mathf.Clamp((float)(newSpeed - tgtSpeed), -Time.deltaTime * 8f, Time.deltaTime * 2f);
var radius = Math.Max(wp.Radius, 10);
if (distance < radius)
{
if (WaypointIndex + 1 >= Waypoints.Count) // last waypoint
{
newSpeed = new [] { newSpeed, (distance < radius * 0.8 ? 0 : 1) }.Min();
// ^ limit speed so it'll only go from 1m/s to full stop when braking to prevent accidents on moons
if (LoopWaypoints)
{
WaypointIndex = 0;
}
else
{
newSpeed = 0;
brake = true;
// tgtSpeed.force(newSpeed);
if (curSpeed < brakeSpeedLimit)
{
if (wp.Quicksave)
{
//if (s.mainThrottle > 0) { s.mainThrottle = 0; }
if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
{
WaypointIndex = -1;
ControlHeading = ControlSpeed = false;
QuickSaveLoad.QuickSave();
}
}
else
{
WaypointIndex = -1;
ControlHeading = ControlSpeed = false;
}
}
else
{
Log.dbg("Is this even getting called?");
// WaypointIndex++;
}
}
}
else
{
if (wp.Quicksave)
{
//if (s.mainThrottle > 0) { s.mainThrottle = 0; }
newSpeed = 0;
// tgtSpeed.force(newSpeed);
if (curSpeed < brakeSpeedLimit)
{
if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
{
WaypointIndex++;
QuickSaveLoad.QuickSave();
}
}
}
else
{
WaypointIndex++;
}
}
}
brake = brake || ((s.wheelThrottle == 0 || !vessel.isActiveVessel) && curSpeed < brakeSpeedLimit && newSpeed < brakeSpeedLimit);
// ^ brake if needed to prevent rolling, hopefully
tgtSpeed = (newSpeed >= 0 ? newSpeed : 0);
}
}
if (ControlHeading)
{
headingPID.intAccum = Mathf.Clamp((float)headingPID.intAccum, -1, 1);
double instantaneousHeading = vesselState.rotationVesselSurface.eulerAngles.y;
headingErr = MuUtils.ClampDegrees180(instantaneousHeading - heading);
if (s.wheelSteer == s.wheelSteerTrim || FlightGlobals.ActiveVessel != vessel)
{
float limit = (Math.Abs(curSpeed) > turnSpeed ? Mathf.Clamp((float)((turnSpeed + 6) / Square(curSpeed)), 0.1f, 1f) : 1f);
// turnSpeed needs to be higher than curSpeed or it will never steer as much as it could even at 0.2m/s above it
// double act = headingPID.Compute(headingErr * headingErr / 10 * Math.Sign(headingErr));
double act = headingPID.Compute(headingErr);
if (traction >= tractionLimit)
{
s.wheelSteer = Mathf.Clamp((float)act, -limit, limit);
// prevents it from flying above a waypoint and landing with steering at max while still going fast
}
}
}
// Brake if there is no controler (Pilot eject from seat)
if (BrakeOnEject && vessel.GetReferenceTransformPart() == null)
{
s.wheelThrottle = 0;
// vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
brake = true;
}
else if (ControlSpeed)
{
speedPID.intAccum = Mathf.Clamp((float)speedPID.intAccum, -5, 5);
speedErr = (WaypointIndex == -1 ? speed.val : tgtSpeed) - Vector3d.Dot(vesselState.surfaceVelocity, vesselState.forward);
if (s.wheelThrottle == s.wheelThrottleTrim || FlightGlobals.ActiveVessel != vessel)
{
float act = (float)speedPID.Compute(speedErr);
s.wheelThrottle = Mathf.Clamp(act, -1f, 1f);
// s.wheelThrottle = (!LimitAcceleration ? Mathf.Clamp(act, -1, 1) : // I think I'm using these ( ? : ) a bit too much
// (traction == 0 ? 0 : (act < 0 ? Mathf.Clamp(act, -1f, 1f) : (lastThrottle + Mathf.Clamp(act - lastThrottle, -0.01f, 0.01f)) * (traction < tractionLimit ? -1 : 1))));
// (lastThrottle + Mathf.Clamp(act, -0.01f, 0.01f)));
#if DEBUG
Log.dbg("Wheel Throttle {0}", s.wheelThrottle + Mathf.Clamp(act, -0.01f, 0.01f));
#endif
if (curSpeed < 0 & s.wheelThrottle < 0)
{
s.wheelThrottle = 0;
} // don't go backwards
if (Mathf.Sign(act) + Mathf.Sign(s.wheelThrottle) == 0)
{
s.wheelThrottle = Mathf.Clamp(act, -1f, 1f);
}
if (speedErr < -1 && StabilityControl && Mathf.Sign(s.wheelThrottle) + Math.Sign(curSpeed) == 0) // StabilityControl && traction > 50 &&
//// vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
{
brake = true;
// foreach (Part p in wheels) {
// if (p.GetModule<ModuleWheels.ModuleWheelDamage>().stressPercent >= 0.01) { // #TODO needs adaptive braking
// brake = false;
// break;
// }
// }
}
//// else if (!StabilityControl || traction <= 50 || speedErr > -0.2 || Mathf.Sign(s.wheelThrottle) + Mathf.Sign((float)curSpeed) != 0) {
//// vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, (GameSettings.BRAKES.GetKey() && vessel.isActiveVessel));
//// }
lastThrottle = Mathf.Clamp(s.wheelThrottle, -1, 1);
}
}
if (StabilityControl)
{
if (!core.attitude.users.Contains(this))
{
core.attitude.users.Add(this);
// line.enabled = true;
}
// float scale = Vector3.Distance(FlightCamera.fetch.mainCamera.transform.position, vessel.CoM) / 900f;
// line.SetPosition(0, vessel.CoM);
// line.SetPosition(1, vessel.CoM + hit.normal * 5);
// line.SetWidth(0, scale + 0.1f);
var fSpeed = (float)curSpeed;
// if (Mathf.Abs(fSpeed) >= turnSpeed * 0.75) {
Vector3 fwd = (Vector3)(traction > 0 ? // V when the speed is low go for the vessels forward, else with a bit of velocity
// ((Mathf.Abs(fSpeed) <= turnSpeed ? vesselState.forward : vesselState.surfaceVelocity / 4) - vessel.transform.right * s.wheelSteer) * Mathf.Sign(fSpeed) :
// // ^ and then add the steering
vesselState.forward * 4 - vessel.transform.right * s.wheelSteer * Mathf.Sign(fSpeed) : // and then add the steering
vesselState.surfaceVelocity); // in the air so follow velocity
Vector3.OrthoNormalize(ref norm, ref fwd);
var quat = Quaternion.LookRotation(fwd, norm);
// if (traction > 0 || speed <= turnSpeed) {
// var u = new Vector3(0, 1, 0);
//
// var q = FlightGlobals.ship_rotation;
// var q_s = quat;
//
// var q_u = new Quaternion(u.x, u.y, u.z, 0);
// var a = Quaternion.Dot(q, q_s * q_u);
// var q_qs = Quaternion.Dot(q, q_s);
// var b = (a == 0) ? Math.Sign(q_qs) : (q_qs / a);
// var g = b / Mathf.Sqrt((b * b) + 1);
// var gu = Mathf.Sqrt(1 - (g * g)) * u;
// var q_d = new Quaternion() { w = g, x = gu.x, y = gu.y, z = gu.z };
// var n = q_s * q_d;
//
// quat = n;
// }
if (vesselState.torqueAvailable.sqrMagnitude > 0)
{
core.attitude.attitudeTo(quat, AttitudeReference.INERTIAL, this);
}
// }
}
if (BrakeOnEnergyDepletion)
{
var batteries = vessel.Parts.FindAll(p => p.Resources.Contains(PartResourceLibrary.ElectricityHashcode) && p.Resources.Get(PartResourceLibrary.ElectricityHashcode).flowState);
var energyLeft = batteries.Sum(p => p.Resources.Get(PartResourceLibrary.ElectricityHashcode).amount) / batteries.Sum(p => p.Resources.Get(PartResourceLibrary.ElectricityHashcode).maxAmount);
var openSolars = vessel.mainBody.atmosphere && // true if in atmosphere and there are breakable solarpanels that aren't broken nor retracted
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.isBreakable && p.deployState != ModuleDeployablePart.DeployState.BROKEN &&
p.deployState != ModuleDeployablePart.DeployState.RETRACTED).Count > 0;
if (openSolars && energyLeft > 0.99)
{
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.isBreakable &&
p.deployState == ModuleDeployablePart.DeployState.EXTENDED).ForEach(p => p.Retract());
}
if (energyLeft < 0.05 && Math.Sign(s.wheelThrottle) + Math.Sign(curSpeed) != 0)
{
s.wheelThrottle = 0;
} // save remaining energy by not using it for acceleration
if (openSolars || energyLeft < 0.03)
{
tgtSpeed = 0;
}
if (curSpeed < brakeSpeedLimit && (energyLeft < 0.05 || openSolars))
{
brake = true;
}
if (curSpeed < 0.1 && energyLeft < 0.05 && !waitingForDaylight &&
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.deployState == ModuleDeployablePart.DeployState.EXTENDED).Count > 0)
{
waitingForDaylight = true;
}
}
// brake = brake && (s.wheelThrottle == 0); // release brake if the user or AP want to drive
if (s.wheelThrottle != 0 && (Math.Sign(s.wheelThrottle) + Math.Sign(curSpeed) != 0 || curSpeed < 1))
{
brake = false; // the AP or user want to drive into the direction of momentum so release the brake
}
if (vessel.isActiveVessel)
{
if (GameSettings.BRAKES.GetKeyUp())
{
brake = false; // release the brakes if the user lets go of them
}
if (GameSettings.BRAKES.GetKey())
{
brake = true; // brake if the user brakes and we aren't about to flip
}
}
tractionLimit = (double)Mathf.Clamp((float)tractionLimit, 0, 100);
vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, brake && (StabilityControl && (ControlHeading || ControlSpeed) ? traction >= tractionLimit : true));
// only let go of the brake when losing traction if the AP is driving, otherwise assume the player knows when to let go of it
// also to not constantly turn off the parking brake from going over a small bump
if (brake && curSpeed < 0.1)
{
s.wheelThrottle = 0;
}
}
// wiring for launchStarted
public void OnLaunch(EventReport report)
{
launchStarted = vesselState.time;
Log.dbg("[MechJebModuleAscentAutopilot] LaunchStarted = {0}", launchStarted);
}
