void WaitUpdate()
{
var bodyVel = data.GetVelocity(bodyRef);
var bodyRot = data.GetRotation(bodyRef);
var localVel = Vector3.Transform(bodyVel, Quaternion.Invert(bodyRot));
var srfRot = data.GetRotation(surfaceRef);
var srfVel = Vector3.Transform(localVel, srfRot);
if (srfVel.X < 0 || vessel.Orbit.TimeToApoapsis < 30)
{
status = Status.Adjust;
}
}
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();
}
}
public override void Update()
{
var bodyVel = data.GetVelocity(bodyRef);
var bodyRot = data.GetRotation(bodyRef);
var localVel = Vector3.Transform(bodyVel, Quaternion.Invert(bodyRot));
var srfRot = data.GetRotation(surfaceRef);
var srfVel = Vector3.Transform(localVel, srfRot);
var velVertical = -srfVel.X;
var bounding = data.GetSurfaceBoundingBox();
var height = (float)Math.Abs(Math.Min(bounding[0].X, bounding[1].X)) + extraHeight;
var thrustDir = Vector3.Transform(new Vector3(0, -1, 0), srfRot);
var maxAcc = (data.GetMaxThrust()) / data.GetMass() * -thrustDir.X;
var alt = data.GetSurfaceAlt() - height;
var needAcc = velVertical * velVertical / 2 / alt + g;
var needThr = needAcc / maxAcc;
LogInfo("height", height.ToString());
LogInfo("alt", alt.ToString());
LogInfo("velv", velVertical.ToString());
LogInfo("maxAcc", maxAcc.ToString());
LogInfo("needThr", needThr.ToString());
if (!on && needThr > 0.5f)
{
on = true;
Log("landing start");
}
if (on)
{
if (velVertical <= 0 || alt <= 0)
{
on = false;
Log("landing end");
vessel.Control.Throttle = 0;
}
else
{
vessel.Control.Throttle = needThr + (needThr - idealThrottle) * 3;
}
}
}
public Vector3 Predict()
{
//var flightSrf = vessel.Flight(vessel.SurfaceReferenceFrame);
var bodyVel = data.GetVelocity(bodyRef);
var bodyRot = data.GetRotation(bodyRef);
var localVel = Vector3.Transform(bodyVel, Quaternion.Invert(bodyRot));
var srfRot = data.GetRotation(surfaceRef);
var srfVel = Vector3.Transform(localVel, srfRot);
//var bounding = vessel.BoundingBox(surfaceRef);
var boundingLocal = data.GetLocalBoundingBox();
var boundingSurface = data.GetSurfaceBoundingBox();
var vesselHeight = Math.Max(Math.Abs(Math.Min(boundingLocal[0].X, boundingLocal[1].X)), Math.Abs(Math.Min(boundingSurface[0].X, boundingSurface[1].X))) + extraHeight;
//var alt = (float)vessel.Flight().SurfaceAltitude - vesselHeight;
//var alt = (float)body.SrfAltitudeAtPosision(Vector3.Zero, surfaceRef) - vesselHeight;
var alt = data.GetSurfaceAlt() - vesselHeight;
//var thrustDir = Vector3.Transform(new Vector3(0, -1, 0), srfRot);
var pos = Vector3.Zero;
var vel = new Vector3(srfVel);
float a = data.GetMaxThrust() / data.GetMass();
float t = 0;
//float vg = (float)Math.Sqrt(2 * alt * g + srfVel.X * srfVel.X);
//float apprT = (vg + srfVel.X) / g;
float apprT = -vel.X / (a - g); //在忽略空气阻力和水平速度的情况下预估出将竖直速度降至零所需的大致时间
float dt = Math.Max(apprT / 10, 0.01f); //递推的步长
int count = 0;
while (vel.X < 0 && count < 30)
{
count++;
t += dt;
vel -= vel.Normalized() * a * dt; //推力的加速
vel.X -= g * dt; //重力的加速
//var drag = flightSrf.SimulateAerodynamicForceAt(body, pos.ToTuple(), vel.ToTuple()).ToVec();
//if(drag.Length > )
//vel += drag / vessel.Mass * dt;
pos += vel * dt;//位移
}
//line.End = pos.ToTuple();
//var estAlt = (float)body.SrfAltitudeAtPosision(pos, surfaceRef) - vesselHeight;// wtf no way to optimize
var estAlt = alt + pos.X; //预计停下的高度
var notSpareRate = -pos.X / (-pos.X + estAlt); //这个比值越接近 1说明剩余的减速空间越小
estT = t;
//LogInfo("srfVel", srfVel.ToString());
LogInfo("alt", alt.ToString() + " m");
LogInfo("iterations", count.ToString());
LogInfo("est.T", apprT.ToString() + " s");
LogInfo("est.burn", t.ToString() + " s");
LogInfo("est.vel", vel.ToString());
//LogInfo("est.position", pos.ToString());
LogInfo("est.alt", estAlt.ToString() + " m");
LogInfo("ratio", notSpareRate.ToString());
LogInfo("Xtra", extraHeight.ToString());
if (!on && alt > 10 && notSpareRate > 0.8f)
{
on = true;
Log("SoftLandingTilt engaged");
}
if (on)
{
if (t < 4.5f && !vessel.Control.Gear)
{
vessel.Control.Gear = true;
}
if (srfVel.X >= 0 || notSpareRate < 0.5f)
{
on = false;
Log("SoftLandingTilt ended");
vessel.Control.Throttle = 0;
}
else
{
vessel.Control.Throttle = notSpareRate + (notSpareRate - 0.8f) * 2;//反馈调节notSpareRate到0.8
}
}
else
{
//vessel.Control.Throttle = 0;
}
return(Vector3.Zero);
}
