public override void Update()
{
if (Input.GetKey(System.Windows.Forms.Keys.K))
{
Log("max");
vessel.Control.Throttle = 1;
return;
}
var targetLocalSurfacePos = -data.GetPosition(hybrid);
var localSurfaceVel = data.GetVelocity(hybrid);
var thrust = data.GetThrust();
var maxThrust = data.GetMaxThrust();
var mass = data.GetMass();
var a = (thrust / mass - 9.81f);
var pitch = vessel.Flight().Pitch;
var posh = targetLocalSurfacePos.X;
var horAdjustFactor = Math.Max(1 - Math.Abs(posh) / 3, 0);
var velh = localSurfaceVel.X;
var poshFuture = posh + velh * future + 0.5f * a * future * future;
var velhFuture = velh + a * future;
integralh = Mathf.Clamp(integralh + posh * (float)Time.gameDeltaTime * horAdjustFactor, -0.1f / Kih, 0.1f / Kih);
var ph = Kph * poshFuture;
var ih = integralh * Kih;
var gFactor = mass * 9.81f / maxThrust / (float)Math.Cos((90 - pitch) * Mathf.Deg2Rad);
var dh = -Kdh * velhFuture;
var targetThrottle = Mathf.Clamp01(ph + ih + dh + gFactor);
var currentThrottle = thrust / maxThrust;
vessel.Control.Throttle = targetThrottle + (targetThrottle - currentThrottle) * 3;
var posHor = targetLocalSurfacePos.Yz;
var velHor = localSurfaceVel.Yz;
integralHor += posHor * (float)Time.gameDeltaTime;
var pHor = KpHor * posHor;
var iHor = integralHor * KiHor * horAdjustFactor;
var dHor = -KdHor * velHor;
var sum = (pHor * horAdjustFactor + iHor * horAdjustFactor + dHor).Clamp(1);
stability.direction = new Vector3(1, sum.X, sum.Y);
if (showLine)
{
line.End = targetLocalSurfacePos.ToTuple();
}
}
