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;
}
}
}
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()
{
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 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);
}
void Correct()
{
//var flight = vessel.Flight(reference);
currentDirection = Vector3.Transform(new Vector3(0, 1, 0), data.GetRotation(reference));
Quaternion invRotation = Quaternion.Invert(data.GetRotation(reference));
Vector3 localTarget = Vector3.Transform(direction, invRotation);
Vector3 localAngularVel = Vector3.Transform(data.GetAngularVelocity(reference), invRotation);
Vector3 localAngularAcc = (localAngularVel - lastLocalAngularVel) / (float)Time.gameDeltaTime;
localExternalAngularAcc = localAngularAcc - lastAppliedAcc;
//Log(localExternalAngularAccFilter.ToString() + "///" + localExternalAngularAcc.ToString() + "///--///" + Time.gameDeltaTime.ToString());
localExternalAngularAccFilter = Vector3.Lerp(localExternalAngularAccFilter, localExternalAngularAcc, (float)Time.gameDeltaTime * 0.2f);//adjustable
Vector3 interia = data.GetInteria();
var torque = data.GetTorque();
localMaxAngularAccP = Vector3.Divide(torque[0], interia);
localMaxAngularAccN = Vector3.Divide(torque[1], interia);
var pitchVel = localAngularVel.X;
var pitchWeight = (float)Math.Sqrt(localTarget.Z * localTarget.Z + localTarget.Y * localTarget.Y);
var pitchTarget = (float)Math.Atan2(localTarget.Z, localTarget.Y);
var pitchBrakeAcc = InX2Acc(-Math.Sign(pitchTarget));
//var pitchForwardAcc = InX2Acc(Math.Sign(pitchTarget));
var pitchTargetVel = DecideVel(pitchTarget, pitchBrakeAcc);
var pitchDiffVel = pitchTargetVel - pitchVel;
var pitchNeedAcc = pitchDiffVel / (float)Time.gameDeltaTime * Kp;
if (pitchWeight > 0.2f)
{
vessel.Control.Pitch = AccX2In(pitchNeedAcc);
}
var yawVel = localAngularVel.Z;
var yawWeight = (float)Math.Sqrt(localTarget.X * localTarget.X + localTarget.Y * localTarget.Y);
var yawTarget = (float)Math.Atan2(-localTarget.X, localTarget.Y);
var yawBrakeAcc = InZ2Acc(-Math.Sign(yawTarget));
var yawTargetVel = DecideVel(yawTarget, yawBrakeAcc);
var yawDiffVel = yawTargetVel - yawVel;
var yawNeedAcc = yawDiffVel / (float)Time.gameDeltaTime * Kp;
if (yawWeight > 0.2f)
{
vessel.Control.Yaw = AccZ2In(yawNeedAcc);
}
if (showline)
{
line.End = (direction * 20).ToTuple();
line.ReferenceFrame = reference;
}
//LogInfo("localDir ", localTarget.ToString());
//LogInfo("localVel ", localAngularVel.ToString());
LogInfo("localTarget", "yaw=" + yawTarget + " pitch=" + pitchTarget);
LogInfo("maxAcc", "pos=" + localMaxAngularAccP.ToString() + " neg=" + localMaxAngularAccN.ToString());
LogInfo("Acc ", "yaw=" + localAngularAcc.Z + " pitch=" + localAngularAcc.X);
LogInfo("ExtAcc", "yaw=" + localExternalAngularAcc.Z + " pitch=" + localExternalAngularAcc.X);
LogInfo("ExtAccf", "yaw=" + localExternalAngularAccFilter.Z + " pitch=" + localExternalAngularAccFilter.X);
LogInfo("targetVel", "yaw=" + yawTargetVel + " pitch=" + pitchTargetVel);
lastLocalAngularVel = localAngularVel;
lastAppliedAcc = Input2Acc(new Vector3(-vessel.Control.Pitch, -vessel.Control.Roll, -vessel.Control.Yaw));
}
