public override void Update()
{
var pos = data.GetPosition(orbitRef);
var rot = data.GetRotation(surfaceRef);
var lon = body.LongitudeAtPosition(pos, orbitRef);
var lat = body.LatitudeAtPosition(pos, orbitRef);
LogInfo("position", pos.ToString());
LogInfo("rotation", rot.ToString());
LogInfo("Lon", lon.ToString());
LogInfo("Lat", lat.ToString());
LogInfo("SrfHeight", body.SurfaceHeight(lat, lon).ToString());
}
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();
}
}
Vector3 TargetPosition(ReferenceFrame referenceFrame)//如果把另一个vessel设为目标（比如回收用的船），则返回这个vessel的位置， 否则返回由给定经纬度和高度确定的位置
{
if (targetVirtual == null)
{
if (target == null)
{
targetFixedPos = body.PositionAtAltitude(Lat, Lon, Height + body.SurfaceHeight(Lat, Lon), bodyRef).ToVec();
targetVirtual = ReferenceFrame.CreateHybrid(connection, ReferenceFrame.CreateRelative(connection, bodyRef, targetFixedPos.ToTuple()), surfaceRef);
}
else
{
targetVirtual = target.SurfaceReferenceFrame;
}
}
//return connection.SpaceCenter().TransformPosition(targetFixedPos.ToTuple(), bodyRef, referenceFrame).ToVec();
return(-data.GetPosition(targetVirtual));
}
