public override void OnFixedUpdate()
{
if (NavBallGuidance && autopilot != null && autopilot.ascentPath != null)
{
double angle = Math.PI / 180 * autopilot.ascentPath.FlightPathAngle(vesselState.altitudeASL, vesselState.speedSurface);
double heading = Math.PI / 180 * OrbitalManeuverCalculator.HeadingForInclination(autopilot.desiredInclination, vesselState.latitude);
Vector3d horizontalDir = Math.Cos(heading) * vesselState.north + Math.Sin(heading) * vesselState.east;
Vector3d dir = Math.Cos(angle) * horizontalDir + Math.Sin(angle) * vesselState.up;
core.target.UpdateDirectionTarget(dir);
}
}
private PontryaginLaunch NewPontryaginForLaunch(double inc, double sma)
{
lambdaDot = Vector3d.zero;
double desiredHeading = OrbitalManeuverCalculator.HeadingForInclination(inc, vesselState.latitude);
Vector3d desiredHeadingVector = Math.Sin(desiredHeading * UtilMath.Deg2Rad) * vesselState.east + Math.Cos(desiredHeading * UtilMath.Deg2Rad) * vesselState.north;
Vector3d desiredThrustVector = Math.Cos(45 * UtilMath.Deg2Rad) * desiredHeadingVector + Math.Sin(45 * UtilMath.Deg2Rad) * vesselState.up; /* 45 pitch guess */
lambda = desiredThrustVector;
Log.info("sma = {0}; deltaV guess = {1}", sma, approximateDeltaV(sma));
return(new PontryaginLaunch(core: core, mu: mainBody.gravParameter, r0: vesselState.orbitalPosition, v0: vesselState.orbitalVelocity, pv0: lambda, dV: approximateDeltaV(sma)));
}
/// <summary>
/// Find the time to a target plane defined by the LAN and inc for a rocket on the ground.
/// </summary>
///
/// <param name="rotationPeriod">Rotation period of the central body (seconds).</param>
/// <param name="latitude">Latitude of the launchite (degrees).</param>
/// <param name="celestialLongitude">Celestial longitude of the current position of the launchsite.</param>
/// <param name="LAN">Longitude of the Ascending Node of the target plane (degrees).</param>
/// <param name="inc">Inclination of the target plane (degrees).</param>
///
public static double TimeToPlane(double rotationPeriod, double latitude, double celestialLongitude, double LAN, double inc)
{
// alpha is the 90 degree angle between the line of longitude and the equator and omitted
double beta = OrbitalManeuverCalculator.HeadingForInclination(inc, latitude) * UtilMath.Deg2Rad;
double c = Math.Abs(latitude) * UtilMath.Deg2Rad; // Abs for south hemisphere launch sites
// b is how many radians to the west of the launch site that the LAN is (east in south hemisphere)
double b = Math.Atan2(2 * Math.Sin(beta), Math.Cos(beta) / Math.Tan(c / 2) + Math.Tan(c / 2) * Math.Cos(beta)); // napier's analogies
// LAN if we launched now
double LANnow = celestialLongitude - Math.Sign(latitude) * b * UtilMath.Rad2Deg;
return(MuUtils.ClampDegrees360(LAN - LANnow) / 360 * rotationPeriod);
}
public override void OnFixedUpdate()
{
if (ascentPath == null)
{
return;
}
if (core.target.Target != null && core.target.Name == TARGET_NAME)
{
double angle = Math.PI / 180 * ascentPath.FlightPathAngle(vesselState.altitudeASL, vesselState.speedSurface);
double heading = Math.PI / 180 * OrbitalManeuverCalculator.HeadingForInclination(desiredInclination, vesselState.latitude);
Vector3d horizontalDir = Math.Cos(heading) * vesselState.north + Math.Sin(heading) * vesselState.east;
Vector3d dir = Math.Cos(angle) * horizontalDir + Math.Sin(angle) * vesselState.up;
core.target.UpdateDirectionTarget(dir);
}
}
public void TargetPeInsertMatchInc(double PeA, double ApA, double inc, bool omitCoast)
{
if (status == PVGStatus.ENABLED)
{
return;
}
bool doupdate = false;
double v0m, r0m, sma;
ConvertToRadVel(PeA, ApA, out r0m, out v0m, out sma);
if (r0m != old_r0m || v0m != old_v0m || inc != old_inc)
{
//Debug.Log("old settings changed");
doupdate = true;
}
if (p == null || doupdate)
{
if (p != null)
{
//Debug.Log("killing a thread if its there to kill");
p.KillThread();
}
//Debug.Log("mainbody.Radius = " + mainBody.Radius);
//Debug.Log("mainbody.gravParameter = " + mainBody.gravParameter);
lambdaDot = Vector3d.zero;
double desiredHeading = OrbitalManeuverCalculator.HeadingForInclination(inc, vesselState.latitude);
Vector3d desiredHeadingVector = Math.Sin(desiredHeading * UtilMath.Deg2Rad) * vesselState.east + Math.Cos(desiredHeading * UtilMath.Deg2Rad) * vesselState.north;
Vector3d desiredThrustVector = Math.Cos(45 * UtilMath.Deg2Rad) * desiredHeadingVector + Math.Sin(45 * UtilMath.Deg2Rad) * vesselState.up; /* 45 pitch guess */
lambda = desiredThrustVector;
PontryaginLaunch solver = new PontryaginLaunch(core: core, mu: mainBody.gravParameter, r0: vesselState.orbitalPosition, v0: vesselState.orbitalVelocity, pv0: lambda.normalized, pr0: Vector3d.zero, dV: v0m);
solver.omitCoast = omitCoast;
solver.flightangle4constraint(r0m, v0m, 0, inc * UtilMath.Deg2Rad);
p = solver;
}
old_v0m = v0m;
old_r0m = r0m;
old_inc = inc;
}
public void TargetPeInsertMatchOrbitPlane(double PeA, double ApA, Orbit o, bool omitCoast)
{
if (status == PVGStatus.ENABLED)
{
return;
}
bool doupdate = false;
double v0m, r0m, sma;
ConvertToRadVel(PeA, ApA, out r0m, out v0m, out sma);
if (r0m != old_r0m || v0m != old_v0m)
{
doupdate = true;
}
if (p == null || doupdate)
{
if (p != null)
{
p.KillThread();
}
lambdaDot = Vector3d.zero;
double desiredHeading = OrbitalManeuverCalculator.HeadingForInclination(o.inclination, vesselState.latitude);
Vector3d desiredHeadingVector = Math.Sin(desiredHeading * UtilMath.Deg2Rad) * vesselState.east + Math.Cos(desiredHeading * UtilMath.Deg2Rad) * vesselState.north;
Vector3d desiredThrustVector = Math.Cos(45 * UtilMath.Deg2Rad) * desiredHeadingVector + Math.Sin(45 * UtilMath.Deg2Rad) * vesselState.up; /* 45 pitch guess */
lambda = desiredThrustVector;
PontryaginLaunch solver = new PontryaginLaunch(core: core, mu: mainBody.gravParameter, r0: vesselState.orbitalPosition, v0: vesselState.orbitalVelocity, pv0: lambda.normalized, pr0: Vector3d.zero, dV: v0m);
solver.omitCoast = omitCoast;
Vector3d pos, vel;
o.GetOrbitalStateVectorsAtUT(vesselState.time, out pos, out vel);
Vector3d h = Vector3d.Cross(pos.xzy, vel.xzy);
double hTm = v0m * r0m; // FIXME: gamma
solver.flightangle5constraint(r0m, v0m, 0, h.normalized * hTm);
p = solver;
Debug.Log("created TargetPeInsertMatchOrbitPlane solver");
}
old_v0m = v0m;
old_r0m = r0m;
}
void DriveGravityTurn(FlightCtrlState s)
{
//stop the gravity turn when our apoapsis reaches the desired altitude
if (autoThrottle && orbit.ApA > desiredOrbitAltitude)
{
mode = AscentMode.COAST_TO_APOAPSIS;
return;
}
//if we've fallen below the turn start altitude, go back to vertical ascent
if (vesselState.altitudeASL < ascentPath.VerticalAscentEnd())
{
mode = AscentMode.VERTICAL_ASCENT;
return;
}
if (autoThrottle)
{
core.thrust.targetThrottle = ThrottleToRaiseApoapsis(orbit.ApR, desiredOrbitAltitude + mainBody.Radius);
if (core.thrust.targetThrottle < 1.0F)
{
//when we are bringing down the throttle to make the apoapsis accurate, we're liable to point in weird
//directions because thrust goes down and so "difficulty" goes up. so just burn prograde
core.attitude.attitudeTo(Vector3d.forward, AttitudeReference.ORBIT, this);
status = "Fine tuning apoapsis";
return;
}
}
//transition gradually from the rotating to the non-rotating reference frame. this calculation ensures that
//when our maximum possible apoapsis, given our orbital energy, is desiredOrbitalRadius, then we are
//fully in the non-rotating reference frame and thus doing the correct calculations to get the right inclination
double GM = mainBody.gravParameter;
double potentialDifferenceWithApoapsis = GM / vesselState.radius - GM / (mainBody.Radius + desiredOrbitAltitude);
double verticalSpeedForDesiredApoapsis = Math.Sqrt(2 * potentialDifferenceWithApoapsis);
double referenceFrameBlend = Mathf.Clamp((float)(vesselState.speedOrbital / verticalSpeedForDesiredApoapsis), 0.0F, 1.0F);
Vector3d actualVelocityUnit = ((1 - referenceFrameBlend) * vesselState.velocityVesselSurfaceUnit
+ referenceFrameBlend * vesselState.velocityVesselOrbitUnit).normalized;
double desiredHeading = Math.PI / 180 * OrbitalManeuverCalculator.HeadingForInclination(desiredInclination, vesselState.latitude);
Vector3d desiredHeadingVector = Math.Sin(desiredHeading) * vesselState.east + Math.Cos(desiredHeading) * vesselState.north;
double desiredFlightPathAngle = ascentPath.FlightPathAngle(vesselState.altitudeASL);
Vector3d desiredVelocityUnit = Math.Cos(desiredFlightPathAngle * Math.PI / 180) * desiredHeadingVector
+ Math.Sin(desiredFlightPathAngle * Math.PI / 180) * vesselState.up;
Vector3d desiredThrustVector = desiredVelocityUnit;
if (correctiveSteering)
{
Vector3d velocityError = (desiredVelocityUnit - actualVelocityUnit);
const double Kp = 5.0; //control gain
//"difficulty" scales the controller gain to account for the difficulty of changing a large velocity vector given our current thrust
double difficulty = vesselState.velocityVesselSurface.magnitude / (50 + 10 * vesselState.ThrustAccel(core.thrust.targetThrottle));
if (difficulty > 5)
{
difficulty = 5;
}
if (vesselState.limitedMaxThrustAccel == 0)
{
difficulty = 1.0; //so we don't freak out over having no thrust between stages
}
Vector3d steerOffset = Kp * difficulty * velocityError;
//limit the amount of steering to 10 degrees. Furthemore, never steer to a FPA of > 90 (that is, never lean backward)
double maxOffset = 10 * Math.PI / 180;
if (desiredFlightPathAngle > 80)
{
maxOffset = (90 - desiredFlightPathAngle) * Math.PI / 180;
}
if (steerOffset.magnitude > maxOffset)
{
steerOffset = maxOffset * steerOffset.normalized;
}
desiredThrustVector += steerOffset;
}
desiredThrustVector = desiredThrustVector.normalized;
core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, this);
status = "Gravity turn";
}
void DriveCoastToApoapsis(FlightCtrlState s)
{
core.thrust.targetThrottle = 0;
double circularSpeed = OrbitalManeuverCalculator.CircularOrbitSpeed(mainBody, orbit.ApR);
double apoapsisSpeed = orbit.SwappedOrbitalVelocityAtUT(orbit.NextApoapsisTime(vesselState.time)).magnitude;
double circularizeBurnTime = (circularSpeed - apoapsisSpeed) / vesselState.limitedMaxThrustAccel;
//Once we get above the atmosphere, plan and execute the circularization maneuver.
//For orbits near the edge of the atmosphere, we can't wait until we break the atmosphere
//to start the burn, so we also compare the timeToAp with the expected circularization burn time.
//if ((vesselState.altitudeASL > mainBody.RealMaxAtmosphereAltitude())
// || (vesselState.limitedMaxThrustAccel > 0 && orbit.timeToAp < circularizeBurnTime / 1.8))
// Sarbian : removed the special case for now. Some ship where turning while still in atmosphere
if (vesselState.altitudeASL > mainBody.RealMaxAtmosphereAltitude())
{
if (core.solarpanel.autodeploySolarPanels)
{
core.solarpanel.ExtendAll();
}
mode = AscentMode.CIRCULARIZE;
core.warp.MinimumWarp();
return;
}
//if our apoapsis has fallen too far, resume the gravity turn
if (orbit.ApA < desiredOrbitAltitude - 1000.0)
{
mode = AscentMode.GRAVITY_TURN;
core.warp.MinimumWarp();
return;
}
//point prograde and thrust gently if our apoapsis falls below the target
//core.attitude.attitudeTo(Vector3d.forward, AttitudeReference.ORBIT, this);
// Actually I have a better idea: Don't initiate orientation changes when there's a chance that our main engine
// might reignite. There won't be enough control authority to counteract that much momentum change.
// - Starwaster
core.thrust.targetThrottle = 0;
double desiredHeading = Math.PI / 180 * OrbitalManeuverCalculator.HeadingForInclination(desiredInclination, vesselState.latitude);
Vector3d desiredHeadingVector = Math.Sin(desiredHeading) * vesselState.east + Math.Cos(desiredHeading) * vesselState.north;
double desiredFlightPathAngle = ascentPath.FlightPathAngle(vesselState.altitudeASL);
Vector3d desiredThrustVector = Math.Cos(desiredFlightPathAngle * Math.PI / 180) * desiredHeadingVector
+ Math.Sin(desiredFlightPathAngle * Math.PI / 180) * vesselState.up;
core.attitude.attitudeTo(desiredThrustVector.normalized, AttitudeReference.INERTIAL, this);
if (autoThrottle && orbit.ApA < desiredOrbitAltitude)
{
core.attitude.attitudeTo(Vector3d.forward, AttitudeReference.INERTIAL, this);
core.thrust.targetThrottle = ThrottleToRaiseApoapsis(orbit.ApR, desiredOrbitAltitude + mainBody.Radius);
}
if (core.node.autowarp)
{
//warp at x2 physical warp:
core.warp.WarpPhysicsAtRate(2);
}
status = "Coasting to edge of atmosphere";
}
