void AdjustUpdate()
{
double utAtHeight = FindHeightT((float)DescendHeight, 1000);
//Vector3 posAtHeight = vessel.Orbit.PositionAt(utAtHeight, vessel.SurfaceReferenceFrame).ToVec();
Vector3 target = TargetPosition(surfaceRef);
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 (data.GetMeanAlt() < 25000 && srfVel.X < 0)
{
status = Status.Descend;//如果高度过低，跳转到Descend
return;
}
Vector3 srfHorVel = new Vector3(srfVel);
srfHorVel.X = 0;
Vector3 targetHor = new Vector3(target);
targetHor.X = 0;
Vector3 targetHorVel = targetHor / (float)(utAtHeight - Time.UT);
Vector3 dv = targetHorVel - srfHorVel;//调至目标轨道所需的水平速度增量
float dvm = dv.Length;
stability.direction = dv;
float errorAngle = Vector3.CalculateAngle(stability.currentDirection, dv);
float maxAcc = data.GetMaxThrust() / data.GetMass();
if (errorAngle < 0.1f)
{
errorTimer += (float)Time.gameDeltaTime;
}
else
{
errorTimer = 0;
}
if (errorTimer > 1 && dvm > 5)
{//稳定朝向目标时打开节流阀，p控制
vessel.Control.Throttle = dvm / maxAcc;
}
else
{
vessel.Control.Throttle = 0;
}
if (dvm < 5)
{
vessel.Control.Throttle = 0;
status = Status.Descend;//足够接近目标轨道，转到Descend
}
LogInfo("dv", dv.ToString());
LogInfo("error", errorAngle.ToString());
}
