private void Done()
{
users.Clear();
ThrustOff();
status = PVGStatus.FINISHED;
enabled = false;
}
public override void OnModuleEnabled()
{
// coast phases are deliberately not reset in Reset() so we never get a completed coast phase again after whacking Reset()
status = PVGStatus.ENABLED;
core.attitude.users.Add(this);
core.thrust.users.Add(this);
core.stageTracking.users.Add(this);
}
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 ( p.threadStart(vesselState.time) )
//Debug.Log("MechJeb: started optimizer thread");
if (status == PVGStatus.INITIALIZING && p.solution != null)
{
status = PVGStatus.CONVERGED;
}
last_optimizer_time = vesselState.time;
}
public override void OnModuleDisabled()
{
core.attitude.attitudeDeactivate();
if (!core.rssMode)
{
core.thrust.ThrustOff();
}
core.thrust.users.Remove(this);
core.stageTracking.users.Remove(this);
status = PVGStatus.FINISHED;
last_success_time = 0.0;
if (p != null)
{
p.KillThread();
}
p = null;
}
public void Reset()
{
// lambda and lambdaDot are deliberately not cleared here
//Debug.Log("call stack: + " + Environment.StackTrace);
if (p != null)
{
p.KillThread();
p = null;
}
status = PVGStatus.INITIALIZING;
last_stage_time = 0.0;
last_optimizer_time = 0.0;
last_success_time = 0.0;
autowarp = false;
if (!MuUtils.PhysicsRunning())
{
core.warp.MinimumWarp();
}
}
private void handle_throttle()
{
if (p == null || p.solution == null)
{
return;
}
if (!allow_execution)
{
return;
}
if (status == PVGStatus.TERMINAL_RCS)
{
RCSOn();
return;
}
PontryaginBase.Arc current_arc = p.solution.arc(vesselState.time);
if (current_arc.thrust != 0)
{
last_burning_stage = current_arc.ksp_stage;
last_burning_stage_complete = current_arc.complete_burn;
}
if (actuallyCoasting())
{
// force RCS on at the state transition
if (!isCoasting())
{
if (!vessel.ActionGroups[KSPActionGroup.RCS])
{
vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, true);
}
}
if (!isTerminalGuidance())
{
if (vesselState.time < last_stage_time + 4)
{
status = PVGStatus.COASTING_STAGING;
}
else
{
status = PVGStatus.COASTING;
}
}
last_coasting_time = vesselState.time;
// this turns off autostaging during the coast (which currently affects fairing separation)
core.staging.autostageLimitInternal = last_burning_stage - 1;
ThrustOff();
}
else
{
if (!isTerminalGuidance())
{
if ((vesselState.time < last_stage_time + 4) || (vesselState.time < last_coasting_time + 4))
{
status = PVGStatus.BURNING_STAGING;
}
else
{
status = PVGStatus.BURNING;
}
}
if (core.staging.autostageLimitInternal > 0)
{
core.staging.autostageLimitInternal = 0;
}
else
{
ThrustOn();
}
}
}
public override void OnFixedUpdate()
{
update_pitch_and_heading();
if (isLoadedPrincipia)
{
// Debug.Log("FOUND PRINCIPIA!!!");
}
if (!HighLogic.LoadedSceneIsFlight)
{
Debug.Log("MechJebModuleGuidanceController [BUG]: PVG enabled in non-flight mode. How does this happen?");
Done();
}
if (!enabled || status == PVGStatus.ENABLED)
{
return;
}
if (status == PVGStatus.FINISHED)
{
Done();
return;
}
if (status == PVGStatus.FAILED)
{
Reset();
}
if (p != null)
{
// propagate exceptions and debug information out of the solver thread
p.Janitorial();
// update the position (safe to call on every tick, will not update if a thread is running)
p.UpdatePosition(vesselState.orbitalPosition, vesselState.orbitalVelocity, lambda, lambdaDot, tgo, vgo);
}
if (p != null && p.solution != null && isTerminalGuidance())
{
bool has_rcs = vessel.hasEnabledRCSModules(); // && ( vesselState.rcsThrustAvailable.up > 0 );
// stopping one tick short is more accurate for rockets without RCS, but sometimes we overshoot with only one tick
int ticks = 1;
if (has_rcs)
{
ticks = 2;
}
if (status == PVGStatus.TERMINAL_RCS && !vessel.ActionGroups[KSPActionGroup.RCS]) // if someone manually disables RCS
{
Done();
return;
}
// bit of a hack to predict velocity + position in the next tick or two
// FIXME: what exactly does KSP do to integrate over timesteps?
Vector3d a0 = vessel.acceleration_immediate - vessel.graviticAcceleration;
double dt = ticks * TimeWarp.fixedDeltaTime;
Vector3d v1 = vesselState.orbitalVelocity + a0 * dt;
Vector3d x1 = vesselState.orbitalPosition + vesselState.orbitalVelocity * dt + 1 / 2 * a0 * dt * dt;
double current = p.znormAtStateVectors(vesselState.orbitalPosition, vesselState.orbitalVelocity);
double future = p.znormAtStateVectors(x1, v1);
if (future > current)
{
if (has_rcs && status == PVGStatus.TERMINAL)
{
status = PVGStatus.TERMINAL_RCS;
if (!vessel.ActionGroups[KSPActionGroup.RCS])
{
vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, true);
}
}
else
{
Done();
return;
}
}
}
handle_throttle();
if (isStaging())
{
return;
}
core.stageTracking.Update();
converge();
}
private void converge()
{
if (p == null)
{
status = PVGStatus.INITIALIZING;
return;
}
if (p.last_success_time > 0)
{
last_success_time = p.last_success_time;
}
// FIXME: should make this use wall clock time rather than simulation time and drop it down to
// 10 seconds or so (phys warp means that this timeout could be 1/4 of the wall time and the
// optimizer may commonly take 3-4 seconds to reconverge in a suboptimal setting -- TF failure case, etc)
if (p.running_time(vesselState.time) > 30)
{
p.KillThread();
p.last_failure_cause = "Optimizer watchdog timeout"; // bit dirty poking other people's data
}
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;
}
// if we have unstable ullage then continuously update the "staging" timer until we are not
if ((vesselState.lowestUllage < VesselState.UllageState.Stable) && !isCoasting())
{
last_stage_time = vesselState.time;
}
if (p.solution != null)
{
// The current_tgo is the "booster" stage of the solution, it is allowed to go negative, for staging freeze
// running the optimizer for 4 seconds on either side of staging.
if (Math.Abs(p.solution.current_tgo(vesselState.time)) < 4)
{
return;
}
// Also if we just triggered a KSP stage separation or just started coasting, then wait for 4 seconds for
// stats to settle before running the optimizer again.
if (vesselState.time < last_stage_time + 4)
{
return;
}
}
p.threadStart(vesselState.time);
//if ( p.threadStart(vesselState.time) )
//Debug.Log("MechJeb: started optimizer thread");
if (status == PVGStatus.INITIALIZING && p.solution != null)
{
status = PVGStatus.CONVERGED;
}
last_optimizer_time = vesselState.time;
}
