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);
}
}
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 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 (ascentPath.IsVerticalAscent(vesselState.altitudeASL, vesselState.speedSurface))
{
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.surfaceVelocity.normalized
+ referenceFrameBlend * vesselState.orbitalVelocity.normalized).normalized;
double desiredHeading = MathExtensions.Deg2Rad * OrbitalManeuverCalculator.HeadingForLaunchInclination(vessel.mainBody, desiredInclination, launchLatitude, OrbitalManeuverCalculator.CircularOrbitSpeed(vessel.mainBody, desiredOrbitAltitude + mainBody.Radius));
Vector3d desiredHeadingVector = Math.Sin(desiredHeading) * vesselState.east + Math.Cos(desiredHeading) * vesselState.north;
double desiredFlightPathAngle = ascentPath.FlightPathAngle(vesselState.altitudeASL, vesselState.speedSurface);
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.surfaceVelocity.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. Furthermore, 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;
if (limitAoA)
{
float fade = vesselState.dynamicPressure < aoALimitFadeoutPressure ? (float)(aoALimitFadeoutPressure / vesselState.dynamicPressure) : 1;
currentMaxAoA = Math.Min(fade * maxAoA, 180d);
limitingAoA = vessel.altitude <mainBody.atmosphereDepth && Vector3.Angle(vesselState.surfaceVelocity, desiredThrustVector)> currentMaxAoA;
if (limitingAoA)
{
desiredThrustVector = Vector3.RotateTowards(vesselState.surfaceVelocity, desiredThrustVector, (float)(currentMaxAoA * Mathf.Deg2Rad), 1).normalized;
}
}
if (forceRoll && Vector3.Angle(vesselState.up, vesselState.forward) > 7 && core.attitude.attitudeError < 5)
{
var pitch = 90 - Vector3.Angle(vesselState.up, desiredThrustVector);
var hdg = core.rover.HeadingToPos(vessel.CoM, vessel.CoM + desiredThrustVector);
core.attitude.attitudeTo(hdg, pitch, turnRoll, this);
}
else
{
core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, this);
}
status = "Gravity turn";
}