// provides AoA limiting and ground track steering to pitch controllers (possibly should be moved into the attitude controller, but // right now it collaborates too heavily with the ascent autopilot) // protected void attitudeTo(double desiredPitch) { double desiredHeading = OrbitalManeuverCalculator.HeadingForLaunchInclination(vessel, vesselState, autopilot.desiredInclination); Vector3d desiredHeadingVector = Math.Sin(desiredHeading * UtilMath.Deg2Rad) * vesselState.east + Math.Cos(desiredHeading * UtilMath.Deg2Rad) * vesselState.north; Vector3d desiredThrustVector = Math.Cos(desiredPitch * UtilMath.Deg2Rad) * desiredHeadingVector + Math.Sin(desiredPitch * UtilMath.Deg2Rad) * vesselState.up; thrustVectorForNavball = desiredThrustVector; desiredThrustVector = desiredThrustVector.normalized; if (autopilot.limitAoA) { float fade = vesselState.dynamicPressure < autopilot.aoALimitFadeoutPressure ? (float)(autopilot.aoALimitFadeoutPressure / vesselState.dynamicPressure) : 1; autopilot.currentMaxAoA = Math.Min(fade * autopilot.maxAoA, 180d); autopilot.limitingAoA = vessel.altitude <mainBody.atmosphereDepth && Vector3.Angle(vesselState.surfaceVelocity, desiredThrustVector)> autopilot.currentMaxAoA; if (autopilot.limitingAoA) { desiredThrustVector = Vector3.RotateTowards(vesselState.surfaceVelocity, desiredThrustVector, (float)(autopilot.currentMaxAoA * Mathf.Deg2Rad), 1).normalized; } } double pitch = 90 - Vector3d.Angle(desiredThrustVector, vesselState.up); double hdg; if (pitch > 89.9) { hdg = desiredHeading; } else { hdg = MuUtils.ClampDegrees360(UtilMath.Rad2Deg * Math.Atan2(Vector3d.Dot(desiredThrustVector, vesselState.east), Vector3d.Dot(desiredThrustVector, vesselState.north))); } if (autopilot.forceRoll) { core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && vesselState.altitudeBottom > 50); if (desiredPitch == 90.0) { core.attitude.attitudeTo(hdg, pitch, autopilot.turnRoll, this); } else { core.attitude.attitudeTo(hdg, pitch, autopilot.verticalRoll, this); } } else { core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, this); } }
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.HeadingForLaunchInclination(vessel.mainBody, autopilot.desiredInclination, launchLatitude, OrbitalManeuverCalculator.CircularOrbitSpeed(vessel.mainBody, autopilot.desiredOrbitAltitude + mainBody.Radius)); 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 override void OnFixedUpdate() { if (NavBallGuidance && autopilot != null && autopilot.ascentPath != null) { double angle = UtilMath.Deg2Rad * autopilot.ascentPath.FlightPathAngle(vesselState.altitudeASL, vesselState.speedSurface); double heading = UtilMath.Deg2Rad * OrbitalManeuverCalculator.HeadingForLaunchInclination(vessel, vesselState, autopilot.desiredInclination); 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 DriveVerticalAscent(FlightCtrlState s) { if (timedLaunch) { status = "Awaiting liftoff"; core.attitude.AxisControl(false, false, false); return; } if (!ascentPath.IsVerticalAscent(vesselState.altitudeASL, vesselState.speedSurface)) { mode = AscentMode.GRAVITY_TURN; } if (autoThrottle && orbit.ApA > desiredOrbitAltitude) { mode = AscentMode.COAST_TO_APOAPSIS; } //during the vertical ascent we just thrust straight up at max throttle if (forceRoll) { // pre-align roll unless correctiveSteering is active as it would just interfere with that double desiredHeading = OrbitalManeuverCalculator.HeadingForLaunchInclination(vessel.mainBody, desiredInclination, launchLatitude, OrbitalManeuverCalculator.CircularOrbitSpeed(vessel.mainBody, desiredOrbitAltitude + mainBody.Radius)); core.attitude.attitudeTo(desiredHeading, 90, verticalRoll, this); } else { core.attitude.attitudeTo(Vector3d.up, AttitudeReference.SURFACE_NORTH, this); } core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && vesselState.altitudeBottom > 50); if (autoThrottle) { core.thrust.targetThrottle = 1.0F; } if (!vessel.LiftedOff() || vessel.Landed) { status = "Awaiting liftoff"; } else { status = "Vertical ascent"; } }
// this provides ground track heading based on desired inclination and is what most consumers should call protected void attitudeTo(double desiredPitch) { double desiredHeading = OrbitalManeuverCalculator.HeadingForLaunchInclination(vessel, vesselState, autopilot.desiredInclination); attitudeTo(desiredPitch, desiredHeading); }
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 (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 = 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 desiredThrustVector = Math.Cos(desiredFlightPathAngle * MathExtensions.Deg2Rad) * desiredHeadingVector + Math.Sin(desiredFlightPathAngle * MathExtensions.Deg2Rad) * 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"; }
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"; }