private void UpdatePredictionPI()
{
// velcity relative to the target is the minus of the velocity relative to the vessel
// (The PID moves the vessel to zero in the target frame)
Omega = -ac.vessel.angularVelocity;
UpdateError();
// see https://archive.is/NqoUm and the "Alt Hold Controller", the acceleration PID is not implemented so we only
// have the first two PIDs in the cascade.
for (int i = 0; i < 3; i++)
{
MaxAlpha[i] = ControlTorque[i] / ac.vesselState.MoI[i];
// scale the LD the user inputs to get the actual LD we use
// (this was determined entirely empirically by seeing that vessels with appx 1000x range of MaxAlpha
// needed a roughly 10x different LD, so by scaling here, we produce a user tunable which should be
// fairly constant across a large range of vessels)
double effLD = LD * Math.Pow(MaxAlpha[i], 1.0 / 3.0);
double Gain = Math.Sqrt(0.5 * MaxAlpha[i] / effLD);
if (Math.Abs(errorVector[i]) <= 2 * effLD)
{
// linear ramp down of acceleration
TargetOmega[i] = Gain * errorVector[i];
}
else
{
// v = - sqrt(2 * F * x / m) is target stopping velocity based on distance
TargetOmega[i] = Math.Sqrt(2 * MaxAlpha[i] * (Math.Abs(errorVector[i]) - effLD)) * Math.Sign(errorVector[i]);
}
if (useStoppingTime)
{
MaxOmega[i] = MaxAlpha[i] * maxStoppingTime;
if (useFlipTime)
{
MaxOmega[i] = Math.Max(MaxOmega[i], Math.PI / minFlipTime);
}
TargetOmega[i] = MuUtils.Clamp(TargetOmega[i], -MaxOmega[i], MaxOmega[i]);
}
}
if (useControlRange && errorTotal * Mathf.Rad2Deg > rollControlRange)
{
TargetOmega[1] = 0;
Pid[1].ResetI();
}
for (int i = 0; i < 3; i++)
{
Pid[i].Ki = Ki;
Pid[i].Kp = Kp / TimeWarp.CurrentRate;
Pid[i].Kd = Kd;
Actuation[i] = Pid[i].Update(Omega[i] / MaxAlpha[i], TargetOmega[i] / MaxAlpha[i], 1.0);
TargetTorque[i] = Actuation[i] * ControlTorque[i]; // for display
}
}
public void UpdatePhi()
{
Transform vesselTransform = ac.vessel.ReferenceTransform;
// 1. The Euler(-90) here is because the unity transform puts "up" as the pointy end, which is wrong. The rotation means that
// "forward" becomes the pointy end, and "up" and "right" correctly define e.g. AoA/pitch and AoS/yaw. This is just KSP being KSP.
// 2. We then use the inverse ship rotation to transform the requested attitude into the ship frame.
Quaternion deltaRotation = Quaternion.Inverse(vesselTransform.transform.rotation * Quaternion.Euler(-90, 0, 0)) * ac.RequestedAttitude;
// get us some euler angles for the target transform
Vector3d ea = deltaRotation.eulerAngles;
double pitch = ea[0] * UtilMath.Deg2Rad;
double yaw = ea[1] * UtilMath.Deg2Rad;
double roll = ea[2] * UtilMath.Deg2Rad;
// law of cosines for the "distance" of the miss in radians
phiTotal = Math.Acos(MuUtils.Clamp(Math.Cos(pitch) * Math.Cos(yaw), -1, 1));
// this is the initial direction of the great circle route of the requested transform
// (pitch is latitude, yaw is -longitude, and we are "navigating" from 0,0)
Vector3d temp = new Vector3d(Math.Sin(pitch), Math.Cos(pitch) * Math.Sin(-yaw), 0);
temp = temp.normalized * phiTotal;
// we assemble phi in the pitch, roll, yaw basis that vessel.MOI uses (right handed basis)
Vector3d phi = new Vector3d(
MuUtils.ClampRadiansPi(temp[0]), // pitch distance around the geodesic
MuUtils.ClampRadiansPi(roll),
MuUtils.ClampRadiansPi(temp[1]) // yaw distance around the geodesic
);
phi.Scale(ac.AxisState);
if (useInertia)
{
phi -= ac.inertia;
}
phiVector = phi;
}
void descend()
{
Quaternion courseCorrection = Quaternion.identity;
// we aim for parachute break point with half deploy time for opening, which we can anticipate
breakDiff = targetInfo.backwardDifference - (core.landing.deployChutes ? 0.5D * parachuteInfo.undeployedDistance : 0);
status = String.Format("Holding planned descent attitude, break diff={0:F1}", breakDiff);
String logs = String.Format("Atmo Correction alt:{0:F0}, speed hor:{1:F0}, AoA: {2:F1}, dist:{3:F0}", vesselState.altitudeASL.value, vesselState.speedSurfaceHorizontal.value, vesselState.AoA.value, targetInfo.forwardDistance);
double backwardCorrection = breakDiff / targetInfo.forwardDistance;
logs += String.Format(", target back:{0:F0} corr:{1:F4} break:{2:F0}", targetInfo.backwardDifference, backwardCorrection, breakDiff);
if (targetInfo.isValid)
{
double AoA = TrajectoriesConnector.API.AoA.Value;
//if we are less than 0°-60° turned on entry or 0-30° otherwise, simply turn more or less to hit target
if (!TrajectoriesConnector.API.isBelowEntry() && Math.Abs(backwardCorrection) > 0.02)
{
AoA = MuUtils.Clamp(AoA - Math.Sign(backwardCorrection) * 0.5, 120d, 180d, out aeroClamp, out _);
if (Math.Abs(TrajectoriesConnector.API.AoA.Value - AoA) > 0.1)
{
TrajectoriesConnector.API.AoA = AoA;
logs += String.Format(", changed AoA=>{0:F1}", AoA);
}
}
else if (Math.Abs(backwardCorrection) > 0.02)
{
AoA = MuUtils.Clamp(AoA - Math.Sign(backwardCorrection) * 0.5, 150d, 180d, out aeroClamp, out _);
if (TrajectoriesConnector.API.AoA != AoA)
{
TrajectoriesConnector.API.AoA = AoA;
logs += String.Format(", changed AoA=>{0:F1}", AoA);
}
}
if (aeroClamp && (core.thrust.targetThrottle != 0 || backwardCorrection > 0.03))
{
core.thrust.targetThrottle = Mathf.Clamp01((float)(gee / vesselState.limitedMaxThrustAccel * backwardCorrection)); // set thrust at 1G for 10% over target => early corrections, but slow enough for Trajectories
logs += String.Format(", reverse thrust=>{0:P0}", core.thrust.targetThrottle);
status += ", +THRUST";
}
else if (!aeroClamp)
{
core.thrust.targetThrottle = 0; // safeguard, actually backwardCorrection should be 0 before Angle gets reduced
}
}
float rollForTarget = 0;
// roll plannedOrientation towards target if we have signifikant lift
if (vesselState.lift > 1)
{
rollForTarget = -Mathf.Asin((float)(2.0 * targetInfo.normalDifference / targetInfo.distanceTarget.magnitude
* vesselState.mass * vesselState.speedSurface / (targetInfo.TimeTillImpact * vesselState.lift))) * Mathf.Rad2Deg;
rollForTarget = Mathf.Clamp(rollForTarget, -30, 30);
}
logs += String.Format(", normalDiff: {0:F0}, time: {1:F0}s, rollAngle={2:F1}", targetInfo.normalDifference, targetInfo.TimeTillImpact, rollForTarget);
if (Mathf.Abs(rollForTarget) < 0.0005 || targetInfo.normalDifference < 50)
{
rollForTarget = 0; //neglect very small differences
}
if (Mathf.Abs(rollForTarget) > 0 && TrajectoriesConnector.API.isBelowEntry())
{
status += String.Format(", roll towards target with {0:F1}", rollForTarget);
courseCorrection = Quaternion.AngleAxis(rollForTarget, Vector3.forward);
}
Debug.Log(logs);
Quaternion plannedOrientation = (Quaternion)TrajectoriesConnector.API.PlannedOrientation();
core.attitude.attitudeTo(courseCorrection * plannedOrientation, AttitudeReference.SURFACE_VELOCITY, this);
}
