public EngineCluster Fastest(Vector3 dV)
{
// Log("Requested fasted cluster for: {}", dV);//debug
var dVm = dV.magnitude;
var loc_dV = VSL.LocalDir(dV);
var min_time = float.MaxValue;
EngineCluster fastest = null;
for (int j = 0, clustersCount = clusters.Count; j < clustersCount; j++)
{
var c = clusters[j];
var time = VSL.Torque.NoEngines.RotationTime(Vector3.Angle(loc_dV, c.Dir), 1);
// Log("Dir {}, Angle {}, Rot.time: {}, coutdown {}", c.Dir, Vector3.Angle(loc_dV, c.Dir), time, VSL.Info.Countdown);//debug
if (VSL.Info.Countdown > 0 && time > VSL.Info.Countdown)
{
continue;
}
time += VSL.Engines.TTB(dVm, c.MaxThrust.magnitude, (float)c.MaxMassFlow, 1);
// Log("MThrust {}, Rot+ttb: {} < min time {}", c.MaxThrust, time, min_time);//debug
if (time < min_time)
{
min_time = time;
fastest = c;
}
}
// Log("Fasted cluster: {}", fastest);//debug
return(fastest ?? Closest(loc_dV));
}
public void Add(EngineWrapper e)
{
Engines.Add(e);
MaxThrust = MaxThrust + VSL.LocalDir(e.defThrustDir) * e.engine.maxThrust;
MaxMassFlow = MaxMassFlow + e.engine.maxFuelFlow;
Dir = MaxThrust.normalized;
}
protected override void correct_steering()
{
if (BRC != null && BRC.IsActive)
{
steering = Vector3.ProjectOnPlane(steering, VSL.LocalDir(VSL.Engines.CurrentDefThrustDir));
}
}
public EngineCluster BestForManeuver(Vector3 dV)
{
var dVm = dV.magnitude;
var loc_dV = -VSL.LocalDir(dV);
var min_score = float.MaxValue;
EngineCluster best = null;
for (int j = 0, clustersCount = clusters.Count; j < clustersCount; j++)
{
var c = clusters[j];
var score = VSL.Torque.NoEngines.RotationTime2Phase(Utils.Angle2(loc_dV, c.Dir), 1);
var thrust = c.MaxThrust.magnitude;
var ttb = VSL.Engines.TTB(dVm, thrust, (float)c.MaxMassFlow, 1);
if (VSL.Info.Countdown > 0 && score + ttb / 2 > VSL.Info.Countdown)
{
continue;
}
if (ttb < ManeuverAutopilot.C.ClosestCluster)
{
ttb = 0;
}
score += ttb;
var fuel = VSL.Engines.FuelNeeded(dVm, (float)(thrust / c.MaxMassFlow));
if (fuel / VSL.Physics.M > ManeuverAutopilot.C.EfficientCluster)
{
score += fuel * ManeuverAutopilot.C.EfficiencyWeight;
}
if (score < min_score)
{
min_score = score;
best = c;
}
}
return(best ?? Closest(loc_dV));
}
public void UpdateAeroForces()
{
var lift = Vector3.zero;
var drag = Vector3.zero;
var torque = Vector3.zero;
if (VSL.vessel.dynamicPressurekPa > 0)
{
for (int i = 0, VSLvesselPartsCount = VSL.vessel.Parts.Count; i < VSLvesselPartsCount; i++)
{
var p = VSL.vessel.Parts[i];
var r = p.Rigidbody.worldCenterOfMass - VSL.Physics.wCoM;
var d = -p.dragVectorDir * p.dragScalar;
torque += Vector3.Cross(r, d);
drag += d;
if (!p.hasLiftModule)
{
var lift_mod = p.transform.rotation * (p.bodyLiftScalar * p.DragCubes.LiftForce);
torque += Vector3.Cross(r, lift_mod);
lift += lift_mod;
}
var lift_srf = p.Modules.GetModules <ModuleLiftingSurface>();
if (lift_srf.Count > 0)
{
var srf_lift = Vector3.zero;
var srf_drag = Vector3.zero;
lift_srf.ForEach(m => { srf_lift += m.liftForce; srf_drag += m.dragForce; });
torque += Vector3.Cross(r, srf_lift + srf_drag);
lift += srf_lift;
drag += srf_drag;
}
}
}
Lift = lift;
Drag = drag;
AeroForce = Lift + Drag;
var relAeroForce = AeroForce / (float)Utils.ClampL(VSL.vessel.dynamicPressurekPa, 1);
if (relAeroForce.sqrMagnitude > MaxAeroForceL.sqrMagnitude)
{
MaxAeroForceL = VSL.LocalDir(relAeroForce);
}
vLift = Vector3.Dot(AeroForce, VSL.Physics.Up);
AeroTorque = torque;
AeroTorqueL = VSL.LocalDir(VSL.OnPlanetParams.AeroTorque);
var aeroTorqueL = AeroTorqueL.AbsComponents();
AeroTorqueRatio = Vector3.Scale(
aeroTorqueL,
(VSL.Torque.NoEngines.Torque + VSL.Torque.EnginesFinal.Torque).Inverse())
.MaxComponentF();
MaxAeroTorqueRatio = Vector3.Scale(
aeroTorqueL,
VSL.Torque.MaxCurrent.Torque.Inverse())
.MaxComponentF();
}
public bool Execute(Vector3d dV, float MinDeltaV = 0.1f, ManeuverCondition condition = null)
{
THR.Throttle = 0;
dVrem.Value = dV.magnitude;
//end if below the minimum dV
if (dVrem < MinDeltaV)
{
return(false);
}
VSL.Engines.ActivateEngines();
//test: flameouted engines are automatically excluded from Active,
//so this should work without this check; but this needs to be tested
//in-game anyway with: MAN, REN, DEO
// if(VSL.Engines.MaxThrustM.Equals(0))
// return VSL.Engines.HaveNextStageEngines;
//orient along the burning vector
if (dVrem && VSL.Controls.RCSAvailableInDirection(-dV))
{
CFG.AT.OnIfNot(Attitude.KillRotation);
}
else
{
CFG.AT.OnIfNot(Attitude.Custom);
ATC.SetThrustDirW(-dV);
VSL.Engines.RequestClusterActivationForManeuver(-dV);
}
//check the condition
if (condition != null && !condition((float)dVrem))
{
return(true);
}
if (VSL.Controls.TranslationAvailable)
{
if (dVrem || VSL.Controls.AttitudeError > GLB.ATCB.AttitudeErrorThreshold)
{
TRA.AddDeltaV(-VSL.LocalDir(dV));
}
if (dVrem && CFG.AT[Attitude.KillRotation])
{
var errorF = Utils.ClampL(Vector3.Dot(VSL.Engines.Thrust.normalized, -dV.normalized), 0);
THR.DeltaV = (float)dVrem * errorF * errorF;
}
else
{
THR.DeltaV = (float)dVrem;
}
}
else
{
THR.DeltaV = (float)dVrem;
}
// Log("\ndVrem: {}\nAttitudeError {}, DeltaV: {}, Throttle {}, RCS {}",
// dVrem, VSL.Controls.AttitudeError, THR.DeltaV, THR.Throttle, VSL.Controls.RCSAvailableInDirection(-dV));//debug
return(true);
}
public void Add(params EngineWrapper[] engines)
{
for (int i = 0, len = engines.Length; i < len; i++)
{
var e = engines[i];
Engines.Add(e);
MaxThrust = MaxThrust + VSL.LocalDir(e.defThrustDir) * e.engine.maxThrust;
MaxMassFlow = MaxMassFlow + e.engine.maxFuelFlow;
}
Dir = MaxThrust.normalized;
}
public bool Execute(Vector3d dV, float MinDeltaV = 0.1f, ManeuverCondition condition = null)
{
THR.Throttle = 0;
dVrem.Value = dV.magnitude;
//end if below the minimum dV
if (dVrem < MinDeltaV)
{
return(false);
}
VSL.Engines.ActivateEngines();
if (VSL.Engines.MaxThrustM.Equals(0))
{
return(true);
}
//orient along the burning vector
if (dVrem && VSL.Controls.RCSAvailableInDirection(-dV))
{
CFG.AT.OnIfNot(Attitude.KillRotation);
}
else
{
CFG.AT.OnIfNot(Attitude.Custom);
ATC.SetThrustDirW(-dV);
}
//check the condition
if (condition != null && !condition((float)dVrem))
{
return(true);
}
if (VSL.Controls.TranslationAvailable)
{
if (dVrem || VSL.Controls.AttitudeError > GLB.ATCB.AttitudeErrorThreshold)
{
TRA.AddDeltaV(-VSL.LocalDir(dV));
}
if (dVrem && CFG.AT[Attitude.KillRotation])
{
var errorF = Utils.ClampL(Vector3.Dot(VSL.Engines.Thrust.normalized, -dV.normalized), 0);
THR.DeltaV = (float)dVrem * errorF * errorF;
}
else
{
THR.DeltaV = (float)dVrem;
}
}
else
{
THR.DeltaV = (float)dVrem;
}
// Log("\ndVrem: {}\nAttitudeError {}, DeltaV: {}, Throttle {}, RCS {}",
// dVrem, VSL.Controls.AttitudeError, THR.DeltaV, THR.Throttle, VSL.Controls.RCSAvailableInDirection(-dV));//debug
return(true);
}
protected override void OnAutopilotUpdate(FlightCtrlState s)
{
//need to check all the prerequisites, because the callback is called asynchroniously
if (!(CFG.Enabled && IsActive && VSL.refT != null))
{
DirectionOverride = Vector3d.zero; return;
}
//allow user to intervene
if (VSL.HasUserInput)
{
if (!s.yaw.Equals(0))
{
UpdateBearing(Bearing.Value + s.yaw * BRC.YawFactor * CFG.ControlSensitivity);
if (CFG.HF[HFlight.CruiseControl] && !VSL.HorizontalSpeed.NeededVector.IsZero())
{
VSL.HorizontalSpeed.SetNeeded(ForwardDirection * CFG.MaxNavSpeed);
}
draw_forward_direction = BRC.DrawForwardDirection;
VSL.HasUserInput = !(s.pitch.Equals(0) && s.roll.Equals(0));
VSL.AutopilotDisabled = VSL.HasUserInput;
DirectionLineTimer.Reset();
bearing_pid.Reset();
s.yaw = 0;
}
}
if (VSL.AutopilotDisabled)
{
DirectionOverride = Vector3d.zero; return;
}
//turn ship's nose in the direction of needed velocity
var axis = up_axis;
var laxis = VSL.LocalDir(axis);
var cDir = H(VSL.OnPlanetParams.Fwd);
var nDir = DirectionOverride.IsZero()? H(ForwardDirection) : H(DirectionOverride);
var angle = Vector3.Angle(cDir, nDir) * Mathf.Sign(Vector3.Dot(Vector3.Cross(nDir, cDir), axis));
var eff = Mathf.Abs(Vector3.Dot(VSL.Engines.CurrentMaxThrustDir.normalized, VSL.Physics.Up));
var ADf = (float)VSL.vessel.staticPressurekPa / VSL.Torque.MaxCurrent.AngularDragResistanceAroundAxis(laxis) * BRC.ADf + 1;
AAf = Utils.Clamp(1 / VSL.Torque.MaxCurrent.AngularAccelerationAroundAxis(laxis) / ADf, BRC.MinAAf, BRC.MaxAAf);
bearing_pid.P = BRC.DirectionPID.P;
bearing_pid.D = BRC.DirectionPID.D * AAf;
bearing_pid.Update(angle * eff, Vector3.Dot(VSL.vessel.angularVelocity, laxis) * Mathf.Rad2Deg);
steering = rotation2steering(world2local_rotation(Quaternion.AngleAxis(bearing_pid.Action, axis)));
VSL.Controls.AddSteering(steering);
// if(VSL.IsActiveVessel)
// TCAGui.DebugMessage =
// Utils.Format("angle {}deg, action {}deg, action*eff {}deg\n" +
// "AAf {}, ADf {}, result {}",
// angle, bearing_pid.Action, bearing_pid.Action*eff,
// 1/VSL.Torque.MaxCurrent.AngularAccelerationAroundAxis(laxis), ADf, AAf);//debug
DirectionOverride = Vector3d.zero;
}
public bool RCSAvailableInDirection(Vector3 wDir)
{
if (VSL.Engines.NumActiveRCS.Equals(0))
{
return(false);
}
var lDir = VSL.LocalDir(wDir).normalized;
var thrust = VSL.Engines.MaxThrustRCS.Project(lDir);
// Utils.Log("\nMaxThrustRCS:\n{}\nRCS dir: {}\nRCS thrust: {}\nRCS accel: {}\nActive RCS: {}\n",
// VSL.Engines.MaxThrustRCS, lDir, thrust, thrust.magnitude/VSL.Physics.M, VSL.Engines.NumActiveRCS);//debug
return(thrust.magnitude / VSL.Physics.M > GLB.TRA.MinDeltaV / 2);
}
public override void Update()
{
var active_dir = Vector3.zero;
//deactivate engines if in SmartEngines mode
if (CFG.UseSmartEngines)
{
//save active direction, if any
if (Active != null)
{
active_dir = VSL.WorldDir(Active.Dir);
}
Deactivate();
}
clusters.Clear();
Active = null;
//repartition enines into clusters
savedRefT = refT;
VSL.Engines.All.Sort((a, b) => b.engine.maxThrust.CompareTo(a.engine.maxThrust));
for (int i = 0, enginesCount = VSL.Engines.All.Count; i < enginesCount; i++)
{
var e = VSL.Engines.All[i];
//do not include maneuver or manual engines into clusters
if (e.Role == TCARole.MANEUVER || e.Role == TCARole.MANUAL)
{
continue;
}
//do not include engines with stack-attached children; these are probably blocked by decouplers
if (e.part.children.Any(ch => ch.srfAttachNode == null || ch.srfAttachNode.attachedPart != e.part))
{
continue;
}
e.UpdateThrustInfo();
float min_dist;
var closest = find_closest(VSL.LocalDir(e.defThrustDir), out min_dist);
if (min_dist < EngineCluster.MaxDistance)
{
closest.Add(e);
}
else
{
clusters.Add(new EngineCluster(VSL, e));
}
}
//activate the cluster that is nearest to the previous active direction
if (CFG.UseSmartEngines && !active_dir.IsZero())
{
activate(Closest(VSL.LocalDir(active_dir)));
}
}
public void RequestClusterActivationForManeuver(Vector3 dV)
{
switch (CFG.SmartEngines.state)
{
case SmartEnginesMode.Closest:
local_dir_cluster_request = -VSL.LocalDir(dV);
break;
case SmartEnginesMode.Fastest:
case SmartEnginesMode.Best:
dV_cluster_request = dV;
break;
}
}
protected override void OnAutopilotUpdate()
{
//allow user to intervene
if (VSL.HasUserInput)
{
if (!CS.yaw.Equals(0))
{
UpdateBearing(Bearing.Value + CS.yaw * C.YawFactor * CFG.ControlSensitivity);
if (CFG.HF[HFlight.CruiseControl] && !VSL.HorizontalSpeed.NeededVector.IsZero())
{
VSL.HorizontalSpeed.SetNeeded(ForwardDirection * CFG.MaxNavSpeed);
}
draw_forward_direction = C.DrawForwardDirection;
VSL.HasUserInput = !(CS.pitch.Equals(0) && CS.roll.Equals(0));
VSL.AutopilotDisabled = VSL.HasUserInput;
DirectionLineTimer.Reset();
Reset();
CS.yaw = 0;
}
}
if (!VSL.AutopilotDisabled)
{
//turn ship's nose in the direction of needed velocity
var nDir = DirectionOverride.IsZero()? H(ForwardDirection) : H(DirectionOverride);
if (!nDir.IsZero())
{
var cDir = VSL.OnPlanetParams.Heading;
var w_axis = VSL.Engines.refT_thrust_axis;
var error_sign = Mathf.Sign(Vector3.Dot(Vector3.Cross(cDir, nDir), w_axis));
rotation_axis = VSL.LocalDir(w_axis) * error_sign;
var angular_error = Utils.Angle2(cDir, nDir) / 180;
var AV = Vector3.Dot(VSL.vessel.angularVelocity, rotation_axis);
var AM = AV * Vector3.Dot(VSL.Physics.MoI, rotation_axis.AbsComponents());
var MaxAA = VSL.Torque.MaxCurrent.AngularAccelerationAroundAxis(rotation_axis);
var iMaxAA = 1 / MaxAA;
pid_yaw.TuneFast(AV, AM, MaxAA, iMaxAA, 1 - angular_error);
pid_yaw.atPID.Update(angular_error * Mathf.PI *
//lower the error when the nose is up or down
(1 - Mathf.Abs(Vector3.Dot(VSL.OnPlanetParams.Fwd, VSL.Physics.Up))), -AV);
steering = rotation_axis * pid_yaw.UpdateAV(AV - pid_yaw.atPID.Action);
VSL.Controls.AddSteering(steering);
}
}
draw_forward_direction = !DirectionLineTimer.TimePassed;
DirectionOverride = Vector3d.zero;
}
public void UpdateCues()
{
switch (CFG.AT.state)
{
case Attitude.Normal:
needed_lthrust = -VSL.LocalDir(VSL.orbit.h.xzy);
break;
case Attitude.AntiNormal:
needed_lthrust = VSL.LocalDir(VSL.orbit.h.xzy);
break;
case Attitude.Radial:
needed_lthrust = VSL.LocalDir(Vector3d.Cross(VSL.vessel.obt_velocity.normalized, VSL.orbit.h.xzy.normalized));
break;
case Attitude.AntiRadial:
needed_lthrust = -VSL.LocalDir(Vector3d.Cross(VSL.vessel.obt_velocity.normalized, VSL.orbit.h.xzy.normalized));
break;
case Attitude.Target:
case Attitude.AntiTarget:
case Attitude.TargetCorrected:
if (!VSL.HasTarget)
{
Message("No target");
CFG.AT.On(Attitude.KillRotation);
break;
}
var dpos = VSL.vessel.transform.position - VSL.Target.GetTransform().position;
if (CFG.AT.state == Attitude.TargetCorrected)
{
var dvel = VSL.vessel.GetObtVelocity() - VSL.Target.GetObtVelocity();
needed_lthrust = VSL.LocalDir((dpos.normalized + Vector3.ProjectOnPlane(dvel, dpos).ClampMagnitudeH(1)).normalized);
}
else
{
needed_lthrust = VSL.LocalDir(dpos.normalized);
if (CFG.AT.state == Attitude.AntiTarget)
{
needed_lthrust *= -1;
}
}
break;
}
}
public EngineCluster BestForManeuver(Vector3 dV)
{
// Log("Requested best cluster for maneuver: {}", dV);//debug
var dVm = dV.magnitude;
var loc_dV = VSL.LocalDir(dV);
var min_score = float.MaxValue;
EngineCluster best = null;
for (int j = 0, clustersCount = clusters.Count; j < clustersCount; j++)
{
var c = clusters[j];
var score = VSL.Torque.NoEngines.RotationTime(Vector3.Angle(loc_dV, c.Dir), 1);
// Log("Dir {}, Angle {}, Rot.time: {}, coutdown {}", c.Dir, Vector3.Angle(loc_dV, c.Dir), score, VSL.Info.Countdown);//debug
var thrust = c.MaxThrust.magnitude;
var ttb = VSL.Engines.TTB(dVm, thrust, (float)c.MaxMassFlow, 1);
if (VSL.Info.Countdown > 0 && score + ttb / 2 > VSL.Info.Countdown)
{
continue;
}
if (ttb < GLB.MAN.ClosestCluster)
{
ttb = 0;
}
score += ttb;
var fuel = VSL.Engines.FuelNeeded(dVm, (float)(thrust / c.MaxMassFlow));
if (fuel / VSL.Physics.M > GLB.MAN.EfficientCluster)
{
score += fuel * GLB.MAN.EfficiencyWeight;
}
// Log("ttb {}, fuel {}, score {} < min score {}", ttb, fuel, score, min_score);//debug
if (score < min_score)
{
min_score = score;
best = c;
}
}
// Log("Best cluster: {}", best);//debug
return(best ?? Closest(loc_dV));
}
// public override void Init()
// {
// base.Init();
// RenderingManager.AddToPostDrawQueue(1, RadarBeam);
// }
//
// public void RadarBeam()
// {
// if(VSL == null || VSL.vessel == null) return;
// for(int i = 0; i < VSL.Engines.NumActive; i++)
// {
// var e = VSL.Engines.Active[i];
// if(e.thrustInfo == null) continue;
// Utils.GLVec(e.wThrustPos, e.wThrustDir * 0.5f, Color.yellow);
// }
// }
//
// public override void Reset()
// {
// base.Reset();
// RenderingManager.RemoveFromPostDrawQueue(1, RadarBeam);
// }
void DebugEngines(IList <EngineWrapper> engines, Vector3 needed_torque)
{
Utils.Log("Engines:\n{}",
engines.Aggregate("", (s, e) => s
+ string.Format("engine(vec{0}, vec{1}, vec{2}, {3}, {4}),\n",
VSL.LocalDir(e.wThrustPos - VSL.Physics.wCoM),
e.thrustDirection, e.specificTorque, e.nominalCurrentThrust(0), e.nominalCurrentThrust(1))));
Utils.Log("Engines Torque:\n{}", engines.Aggregate("", (s, e) => s + "vec" + e.Torque(e.throttle * e.limit) + ",\n"));
Utils.Log(
"Steering: {}\n" +
"Needed Torque: {}\n" +
"Torque Imbalance: {}\n" +
"Torque Error: {}kNm, {}°\n" +
"Torque Clamp:\n +{}\n -{}\n" +
"Limits: [{}]",
Steering,
needed_torque,
engines.Aggregate(Vector3.zero, (v, e) => v + e.Torque(e.throttle * e.limit)),
TorqueError, TorqueAngle,
VSL.Torque.EnginesLimits.positive,
VSL.Torque.EnginesLimits.negative,
engines.Aggregate("", (s, e) => s + e.limit + " ").Trim()
);
}
public EngineCluster Fastest(Vector3 dV)
{
var dVm = dV.magnitude;
var loc_dir = -VSL.LocalDir(dV);
var min_time = float.MaxValue;
EngineCluster fastest = null;
for (int j = 0, clustersCount = clusters.Count; j < clustersCount; j++)
{
var c = clusters[j];
var time = VSL.Torque.NoEngines.RotationTime2Phase(Utils.Angle2(loc_dir, c.Dir), 1);
if (VSL.Info.Countdown > 0 && time > VSL.Info.Countdown)
{
continue;
}
time += VSL.Engines.TTB(dVm, c.MaxThrust.magnitude, (float)c.MaxMassFlow, 1);
if (time < min_time)
{
min_time = time;
fastest = c;
}
}
return(fastest ?? Closest(loc_dir));
}
protected override Vector3 get_angular_velocity()
{
return(CFG.BR ?
Vector3.ProjectOnPlane(VSL.vessel.angularVelocity, VSL.LocalDir(VSL.Engines.refT_thrust_axis)) :
VSL.vessel.angularVelocity);
}
public void UpdateCues()
{
switch (CFG.AT.state)
{
case Attitude.Normal:
needed_lthrust = -VSL.LocalDir(VSL.orbit.h.xzy);
break;
case Attitude.AntiNormal:
needed_lthrust = VSL.LocalDir(VSL.orbit.h.xzy);
break;
case Attitude.Radial:
needed_lthrust = VSL.LocalDir(Vector3d.Cross(VSL.vessel.obt_velocity.normalized, VSL.orbit.h.xzy.normalized));
break;
case Attitude.AntiRadial:
needed_lthrust = -VSL.LocalDir(Vector3d.Cross(VSL.vessel.obt_velocity.normalized, VSL.orbit.h.xzy.normalized));
break;
case Attitude.Target:
case Attitude.AntiTarget:
case Attitude.TargetCorrected:
Vector3 dpos;
try
{
if (!CFG.Target.Valid)
{
throw new NullReferenceException();
}
dpos = VSL.vessel.transform.position - CFG.Target.GetTransform().position;
}
catch (NullReferenceException)
{
SetTarget();
CFG.AT.On(Attitude.KillRotation);
Message("No target");
break;
}
if (CFG.AT.state == Attitude.TargetCorrected)
{
var dvel = VSL.vessel.GetObtVelocity() - VSL.Target.GetObtVelocity();
needed_lthrust = VSL.LocalDir((dpos.normalized + Vector3.ProjectOnPlane(dvel, dpos).ClampMagnitudeH(1)).normalized);
}
else
{
needed_lthrust = VSL.LocalDir(dpos.normalized);
if (CFG.AT.state == Attitude.AntiTarget)
{
needed_lthrust *= -1;
}
}
break;
}
#if DEBUG
if (FollowMouse)
{
needed_lthrust = VSL.LocalDir(FlightCamera.fetch.mainCamera.ScreenPointToRay(Input.mousePosition).direction);
}
#endif
}
protected override void Update()
{
if (!IsActive || CFG.Target == null || CFG.Nav.Paused)
{
return;
}
CFG.Target.Update(VSL);
//update things that are needed to fly in formation
if (CFG.Nav[Navigation.FollowTarget])
{
update_formation_info();
}
else
{
tVSL = null; tPN = null;
}
//calculate direct distance
var vdir = Vector3.ProjectOnPlane(CFG.Target.GetTransform().position + formation_offset - VSL.Physics.wCoM, VSL.Physics.Up);
var distance = Utils.ClampL(vdir.magnitude - VSL.Geometry.R, 0);
//update destination
if (tPN != null && !tPN.VSL.Info.Destination.IsZero())
{
VSL.Info.Destination = tPN.VSL.Info.Destination;
}
else
{
VSL.Info.Destination = vdir;
}
//handle flying in formation
var tvel = Vector3.zero;
var vel_is_set = false;
var end_distance = CFG.Target.AbsRadius;
if (CFG.Target.Land)
{
end_distance /= 4;
}
var dvel = VSL.HorizontalSpeed.Vector;
if (tVSL != null && tVSL.loaded)
{
if (formation_offset.IsZero())
{
end_distance *= all_followers.Count / 2f;
}
tvel = Vector3d.Exclude(VSL.Physics.Up, tVSL.srf_velocity);
dvel -= tvel;
var tvel_m = tvel.magnitude;
var dir2vel_cos = Vector3.Dot(vdir.normalized, tvel.normalized);
var lat_dir = Vector3.ProjectOnPlane(vdir - VSL.HorizontalSpeed.Vector * PN.LookAheadTime, tvel);
var lat_dist = lat_dir.magnitude;
FormationBreakTimer.RunIf(() => keep_formation = false,
tvel_m < PN.FormationSpeedCutoff);
Maneuvering = CanManeuver && lat_dist > CFG.Target.AbsRadius && distance < CFG.Target.AbsRadius * 3;
if (keep_formation && tvel_m > 0 &&
(!CanManeuver ||
dir2vel_cos <= PN.BearingCutoffCos ||
lat_dist < CFG.Target.AbsRadius * 3))
{
if (CanManeuver)
{
HSC.AddWeightedCorrection(lat_dir.normalized * Utils.ClampH(lat_dist / CFG.Target.AbsRadius, 1) *
tvel_m * PN.FormationFactor * (Maneuvering? 1 : 0.5f));
}
distance = Utils.ClampL(Mathf.Abs(dir2vel_cos) * distance - VSL.Geometry.R, 0);
if (dir2vel_cos < 0)
{
if (distance < CFG.Target.AbsRadius)
{
HSC.AddRawCorrection(tvel * Utils.Clamp(-distance / CFG.Target.AbsRadius * PN.FormationFactor, -PN.FormationFactor, 0));
}
else if (Vector3.Dot(vdir, dvel) < 0 &&
(dvel.magnitude > PN.FollowerMaxAwaySpeed ||
distance > CFG.Target.AbsRadius * 5))
{
keep_formation = true;
VSL.HorizontalSpeed.SetNeeded(vdir);
return;
}
else
{
HSC.AddRawCorrection(tvel * (PN.FormationFactor - 1));
}
distance = 0;
}
vdir = tvel;
}
}
//if the distance is greater that the threshold (in radians), use Great Circle navigation
if (distance / VSL.Body.Radius > PN.DirectNavThreshold)
{
var next = CFG.Target.PointFrom(VSL.vessel, 0.1);
distance = (float)CFG.Target.DistanceTo(VSL.vessel);
vdir = Vector3.ProjectOnPlane(VSL.Body.GetWorldSurfacePosition(next.Lat, next.Lon, VSL.vessel.altitude)
- VSL.vessel.transform.position, VSL.Physics.Up);
tvel = Vector3.zero;
}
else if (!VSL.IsActiveVessel && distance > GLB.UnpackDistance)
{
VSL.SetUnpackDistance(distance * 1.2f);
}
vdir.Normalize();
//check if we have arrived to the target and stayed long enough
if (distance < end_distance)
{
var prev_needed_speed = VSL.HorizontalSpeed.NeededVector.magnitude;
if (prev_needed_speed < 1 && !CFG.HF[HFlight.Move])
{
CFG.HF.OnIfNot(HFlight.Move);
}
else if (prev_needed_speed > 10 && !CFG.HF[HFlight.NoseOnCourse])
{
CFG.HF.OnIfNot(HFlight.NoseOnCourse);
}
if (CFG.Nav[Navigation.FollowTarget])
{
if (tvel.sqrMagnitude > 1)
{
//set needed velocity and starboard to match that of the target
keep_formation = true;
VSL.HorizontalSpeed.SetNeeded(tvel);
HSC.AddRawCorrection((tvel - VSL.HorizontalSpeed.Vector) * 0.9f);
}
else
{
VSL.HorizontalSpeed.SetNeeded(Vector3d.zero);
}
vel_is_set = true;
}
else if (CFG.Nav[Navigation.FollowPath] && CFG.Waypoints.Count > 0)
{
if (CFG.Waypoints.Peek() == CFG.Target)
{
if (CFG.Waypoints.Count > 1)
{
CFG.Waypoints.Dequeue();
if (on_arrival())
{
return;
}
start_to(CFG.Waypoints.Peek());
return;
}
else if (ArrivedTimer.TimePassed)
{
CFG.Waypoints.Clear();
if (on_arrival())
{
return;
}
finish();
return;
}
}
else
{
if (on_arrival())
{
return;
}
start_to(CFG.Waypoints.Peek());
return;
}
}
else if (ArrivedTimer.TimePassed)
{
if (on_arrival())
{
return;
}
finish(); return;
}
}
else
{
CFG.HF.OnIfNot(HFlight.NoseOnCourse);
ArrivedTimer.Reset();
}
//if we need to make a sharp turn, stop and turn, then go on
if (Vector3.Dot(vdir, VSL.OnPlanetParams.Fwd) < PN.BearingCutoffCos &&
Vector3d.Dot(VSL.HorizontalSpeed.normalized, vdir) < PN.BearingCutoffCos)
{
VSL.HorizontalSpeed.SetNeeded(vdir);
CFG.HF.OnIfNot(HFlight.NoseOnCourse);
Maneuvering = false;
vel_is_set = true;
}
var cur_vel = (float)Vector3d.Dot(dvel, vdir);
if (!vel_is_set)
{
//don't slow down on intermediate waypoints too much
var min_dist = PN.OnPathMinDistance * VSL.Geometry.R;
if (!CFG.Target.Land && CFG.Nav[Navigation.FollowPath] &&
CFG.Waypoints.Count > 1 && distance < min_dist)
{
WayPoint next_wp = null;
if (CFG.Waypoints.Peek() == CFG.Target)
{
var iwp = CFG.Waypoints.GetEnumerator();
try
{
iwp.MoveNext(); iwp.MoveNext();
next_wp = iwp.Current;
}
catch {}
}
else
{
next_wp = CFG.Waypoints.Peek();
}
if (next_wp != null)
{
next_wp.Update(VSL);
var next_dist = Vector3.ProjectOnPlane(next_wp.GetTransform().position - CFG.Target.GetTransform().position, VSL.Physics.Up);
var angle2next = Vector3.Angle(vdir, next_dist);
var minD = Utils.ClampL(min_dist * (1 - angle2next / 180 / VSL.Torque.MaxPitchRoll.AA_rad * PN.PitchRollAAf), CFG.Target.AbsRadius);
if (minD > distance)
{
distance = minD;
}
}
else
{
distance = min_dist;
}
}
else if (CFG.Nav.Not(Navigation.FollowTarget))
{
distance = Utils.ClampL(distance - end_distance + VSL.Geometry.D, 0);
}
//tune maximum speed and PID
if (CFG.MaxNavSpeed < 10)
{
CFG.MaxNavSpeed = 10;
}
DistancePID.Min = 0;
DistancePID.Max = CFG.MaxNavSpeed;
if (cur_vel > 0)
{
var brake_thrust = Mathf.Max(VSL.Engines.ManualThrustLimits.Project(VSL.LocalDir(vdir)).magnitude,
VSL.Physics.mg * VSL.OnPlanetParams.TWRf);
var eta = distance / cur_vel;
var max_speed = 0f;
if (brake_thrust > 0)
{
var brake_accel = brake_thrust / VSL.Physics.M;
var brake_time = cur_vel / brake_accel;
max_speed = brake_accel * eta;
if (eta <= brake_time * PN.BrakeOffset)
{
HSC.AddRawCorrection((eta / brake_time / PN.BrakeOffset - 1) * VSL.HorizontalSpeed.Vector);
}
}
if (max_speed < CFG.MaxNavSpeed)
{
DistancePID.Max = max_speed;
}
}
//update the needed velocity
DistancePID.Update(distance * PN.DistanceFactor);
var nV = vdir * DistancePID.Action;
//correcto for Follow Target program
if (CFG.Nav[Navigation.FollowTarget] && Vector3d.Dot(tvel, vdir) > 0)
{
nV += tvel;
}
VSL.HorizontalSpeed.SetNeeded(nV);
}
//correct for lateral movement
var latV = -Vector3d.Exclude(vdir, VSL.HorizontalSpeed.Vector);
var latF = (float)Math.Min((latV.magnitude / Math.Max(VSL.HorizontalSpeed.Absolute, 0.1)), 1);
LateralPID.P = PN.LateralPID.P * latF;
LateralPID.I = Mathf.Min(PN.LateralPID.I, latF);
LateralPID.D = PN.LateralPID.D * latF;
LateralPID.Update(latV);
HSC.AddWeightedCorrection(LateralPID.Action);
// LogF("\ndir v {}\nlat v {}\nact v {}\nlatPID {}",
// VSL.HorizontalSpeed.NeededVector, latV,
// LateralPID.Action, LateralPID);//debug
}
protected override void Update()
{
if (CFG.Nav.Paused)
{
return;
}
//differentiate between flying in formation and going to the target
var vdistance = 0f; //vertical distance to the target
if (CFG.Nav[Navigation.FollowTarget])
{
update_formation_info();
}
else
{
VSL.Altitude.LowerThreshold = (float)CFG.Target.Pos.Alt;
if (ALT != null && CFG.VF[VFlight.AltitudeControl] && CFG.AltitudeAboveTerrain)
{
vdistance = VSL.Altitude.LowerThreshold + CFG.DesiredAltitude - VSL.Altitude.Absolute;
}
tVSL = null;
tPN = null;
}
//calculate direct distance
var vdir = Vector3.ProjectOnPlane(CFG.Target.GetTransform().position + formation_offset - VSL.Physics.wCoM, VSL.Physics.Up);
var hdistance = Utils.ClampL(vdir.magnitude - VSL.Geometry.R, 0);
var bearing_threshold = Utils.Clamp(1 / VSL.Torque.MaxCurrent.AngularAccelerationAroundAxis(VSL.Engines.CurrentDefThrustDir),
C.BearingCutoffCos, 0.98480775f); //10deg yaw error
//update destination
if (tPN != null && tPN.Valid && !tPN.VSL.Info.Destination.IsZero())
{
VSL.Info.Destination = tPN.VSL.Info.Destination;
}
else
{
VSL.Info.Destination = vdir;
}
//handle flying in formation
var tvel = Vector3.zero;
var vel_is_set = false;
var end_distance = CFG.Target.AbsRadius;
var dvel = VSL.HorizontalSpeed.Vector;
if (CFG.Target.Land)
{
end_distance /= 4;
}
if (tVSL != null && tVSL.loaded)
{
if (formation_offset.IsZero())
{
end_distance *= all_followers.Count / 2f;
}
tvel = Vector3d.Exclude(VSL.Physics.Up, tVSL.srf_velocity);
dvel -= tvel;
var tvel_m = tvel.magnitude;
var dir2vel_cos = Vector3.Dot(vdir.normalized, tvel.normalized);
var lat_dir = Vector3.ProjectOnPlane(vdir - VSL.HorizontalSpeed.Vector * C.LookAheadTime, tvel);
var lat_dist = lat_dir.magnitude;
FormationBreakTimer.RunIf(() => keep_formation = false,
tvel_m < C.FormationSpeedCutoff);
Maneuvering = CanManeuver && lat_dist > CFG.Target.AbsRadius && hdistance < CFG.Target.AbsRadius * 3;
if (keep_formation && tvel_m > 0 &&
(!CanManeuver ||
dir2vel_cos <= bearing_threshold ||
lat_dist < CFG.Target.AbsRadius * 3))
{
if (CanManeuver)
{
HSC.AddWeightedCorrection(lat_dir.normalized * Utils.ClampH(lat_dist / CFG.Target.AbsRadius, 1) *
tvel_m * C.FormationFactor * (Maneuvering? 1 : 0.5f));
}
hdistance = Utils.ClampL(Mathf.Abs(dir2vel_cos) * hdistance - VSL.Geometry.R, 0);
if (dir2vel_cos < 0)
{
if (hdistance < CFG.Target.AbsRadius)
{
HSC.AddRawCorrection(tvel * Utils.Clamp(-hdistance / CFG.Target.AbsRadius * C.FormationFactor, -C.FormationFactor, 0));
}
else if (Vector3.Dot(vdir, dvel) < 0 &&
(dvel.magnitude > C.FollowerMaxAwaySpeed ||
hdistance > CFG.Target.AbsRadius * 5))
{
keep_formation = true;
VSL.HorizontalSpeed.SetNeeded(vdir);
return;
}
else
{
HSC.AddRawCorrection(tvel * (C.FormationFactor - 1));
}
hdistance = 0;
}
vdir = tvel;
}
}
//if the distance is greater that the threshold (in radians), use the Great Circle navigation
if (hdistance / VSL.Body.Radius > C.DirectNavThreshold)
{
var next = CFG.Target.PointFrom(VSL.vessel, 0.1);
hdistance = (float)CFG.Target.DistanceTo(VSL.vessel);
vdir = Vector3.ProjectOnPlane(VSL.Body.GetWorldSurfacePosition(next.Lat, next.Lon, VSL.vessel.altitude)
- VSL.vessel.transform.position, VSL.Physics.Up);
tvel = Vector3.zero;
}
else if (!VSL.IsActiveVessel && hdistance > GLB.UnpackDistance)
{
VSL.SetUnpackDistance(hdistance * 1.2f);
}
vdir.Normalize();
//check if we have arrived to the target and stayed long enough
if (hdistance < end_distance)
{
if (CFG.Nav[Navigation.FollowTarget])
{
var prev_needed_speed = VSL.HorizontalSpeed.NeededVector.magnitude;
if (prev_needed_speed < 1 && !CFG.HF[HFlight.Move])
{
CFG.HF.OnIfNot(HFlight.Move);
}
else if (prev_needed_speed > 10 && !CFG.HF[HFlight.NoseOnCourse])
{
CFG.HF.OnIfNot(HFlight.NoseOnCourse);
}
if (tvel.sqrMagnitude > 1)
{
//set needed velocity and starboard to match that of the target
keep_formation = true;
VSL.HorizontalSpeed.SetNeeded(tvel);
HSC.AddRawCorrection((tvel - VSL.HorizontalSpeed.Vector) * 0.9f);
}
else
{
VSL.HorizontalSpeed.SetNeeded(Vector3d.zero);
}
vel_is_set = true;
}
else
{
CFG.HF.OnIfNot(HFlight.Move);
VSL.Altitude.DontCorrectIfSlow();
VSL.HorizontalSpeed.SetNeeded(Vector3d.zero);
vel_is_set = true;
if (vdistance <= 0 && vdistance > -VSL.Geometry.R)
{
if (CFG.Nav[Navigation.FollowPath] && CFG.Path.Count > 0)
{
if (CFG.Path.Peek() == CFG.Target)
{
if (CFG.Path.Count > 1)
{
CFG.Path.Dequeue();
if (on_arrival())
{
return;
}
start_to(CFG.Path.Peek());
return;
}
if (ArrivedTimer.TimePassed)
{
CFG.Path.Clear();
if (on_arrival())
{
return;
}
anchor_at_target();
return;
}
}
else
{
if (on_arrival())
{
return;
}
start_to(CFG.Path.Peek());
return;
}
}
else if (ArrivedTimer.TimePassed)
{
if (on_arrival())
{
return;
}
finish();
return;
}
}
}
}
else
{
ArrivedTimer.Reset();
CFG.HF.OnIfNot(HFlight.NoseOnCourse);
//if we need to make a sharp turn, stop and turn, then go on
var heading_dir = Vector3.Dot(VSL.OnPlanetParams.Heading, vdir);
var hvel_dir = Vector3d.Dot(VSL.HorizontalSpeed.normalized, vdir);
var sharp_turn_allowed = !CFG.Nav[Navigation.FollowTarget] ||
hdistance < end_distance *Utils.ClampL(all_followers.Count / 2, 2);
if (heading_dir < bearing_threshold &&
hvel_dir < bearing_threshold &&
sharp_turn_allowed)
{
SharpTurnTimer.Start();
}
if (SharpTurnTimer.Started)
{
VSL.HorizontalSpeed.SetNeeded(vdir);
Maneuvering = false;
vel_is_set = true;
if (heading_dir < bearing_threshold || sharp_turn_allowed &&
VSL.HorizontalSpeed.Absolute > 1 && Math.Abs(hvel_dir) < C.BearingCutoffCos)
{
SharpTurnTimer.Restart();
}
else if (SharpTurnTimer.TimePassed)
{
SharpTurnTimer.Reset();
}
}
// Log("timer: {}\nheading*dir {} < {}, vel {} > 1, vel*dir {} < {}",
// SharpTurnTimer,
// Vector3.Dot(VSL.OnPlanetParams.Heading, vdir), bearing_threshold,
// VSL.HorizontalSpeed.Absolute, Vector3d.Dot(VSL.HorizontalSpeed.normalized, vdir), bearing_threshold);//debug
}
var cur_vel = (float)Vector3d.Dot(dvel, vdir);
if (!vel_is_set)
{
//don't slow down on intermediate waypoints too much
var min_dist = C.OnPathMinDistance * VSL.Geometry.R;
if (!CFG.Target.Land && CFG.Nav[Navigation.FollowPath] &&
CFG.Path.Count > 1 && hdistance < min_dist)
{
WayPoint next_wp = null;
if (CFG.Path.Peek() == CFG.Target)
{
using (var iwp = CFG.Path.GetEnumerator())
{
if (iwp.MoveNext() &&
iwp.MoveNext())
{
next_wp = iwp.Current;
}
}
}
else
{
next_wp = CFG.Path.Peek();
}
if (next_wp != null)
{
next_wp.Update(VSL);
var next_dist = Vector3.ProjectOnPlane(next_wp.GetTransform().position - CFG.Target.GetTransform().position, VSL.Physics.Up);
var angle2next = Utils.Angle2(vdir, next_dist);
var minD = Utils.ClampL(min_dist * (1 - angle2next / 180 / VSL.Torque.MaxPitchRoll.AA_rad * C.PitchRollAAf), CFG.Target.AbsRadius);
if (minD > hdistance)
{
hdistance = minD;
}
}
else
{
hdistance = min_dist;
}
}
else if (CFG.Nav.Not(Navigation.FollowTarget))
{
hdistance = Utils.ClampL(hdistance - end_distance + VSL.Geometry.D, 0);
}
//tune maximum speed and PID
if (CFG.MaxNavSpeed < 10)
{
CFG.MaxNavSpeed = 10;
}
DistancePID.Min = HorizontalSpeedControl.C.TranslationMinDeltaV + 0.1f;
DistancePID.Max = CFG.MaxNavSpeed;
if (CFG.Nav[Navigation.FollowTarget])
{
DistancePID.P = C.DistancePID.P / 2;
DistancePID.D = DistancePID.P / 2;
}
else
{
DistancePID.P = C.DistancePID.P;
DistancePID.D = C.DistancePID.D;
}
if (cur_vel > 0)
{
var mg2 = VSL.Physics.mg * VSL.Physics.mg;
var brake_thrust = Mathf.Min(VSL.Physics.mg, VSL.Engines.MaxThrustM / 2 * VSL.OnPlanetParams.TWRf);
var max_thrust = Mathf.Min(Mathf.Sqrt(brake_thrust * brake_thrust + mg2), VSL.Engines.MaxThrustM * 0.99f);
var horizontal_thrust = VSL.Engines.TranslationThrustLimits.Project(VSL.LocalDir(vdir)).magnitude;
if (horizontal_thrust > brake_thrust)
{
brake_thrust = horizontal_thrust;
}
else
{
horizontal_thrust = -1;
}
if (brake_thrust > 0)
{
var brake_accel = brake_thrust / VSL.Physics.M;
var prep_time = 0f;
if (horizontal_thrust < 0)
{
var brake_angle = Utils.Angle2(VSL.Engines.CurrentDefThrustDir, vdir) - 45;
if (brake_angle > 0)
{
//count rotation of the vessel to braking position
var axis = Vector3.Cross(VSL.Engines.CurrentDefThrustDir, vdir);
if (VSL.Torque.Slow)
{
prep_time = VSL.Torque.NoEngines.RotationTime3Phase(brake_angle, axis, C.RotationAccelPhase);
//also count time needed for the engines to get to full thrust
prep_time += Utils.LerpTime(VSL.Engines.Thrust.magnitude, VSL.Engines.MaxThrustM, max_thrust, VSL.Engines.AccelerationSpeed);
}
else
{
prep_time = VSL.Torque.MaxCurrent.RotationTime2Phase(brake_angle, axis, VSL.OnPlanetParams.GeeVSF);
}
}
}
var prep_dist = cur_vel * prep_time + CFG.Target.AbsRadius;
var eta = hdistance / cur_vel;
max_speed.TauUp = C.MaxSpeedFilterUp / eta / brake_accel;
max_speed.TauDown = eta * brake_accel / C.MaxSpeedFilterDown;
max_speed.Update(prep_dist < hdistance?
(1 + Mathf.Sqrt(1 + 2 / brake_accel * (hdistance - prep_dist))) * brake_accel :
2 * brake_accel);
CorrectionPID.Min = -VSL.HorizontalSpeed.Absolute;
if (max_speed < cur_vel)
{
CorrectionPID.Update(max_speed - cur_vel);
}
else
{
CorrectionPID.IntegralError *= (1 - TimeWarp.fixedDeltaTime * C.CorrectionEasingRate);
CorrectionPID.Update(0);
}
HSC.AddRawCorrection(CorrectionPID.Action * VSL.HorizontalSpeed.Vector.normalized);
}
if (max_speed < CFG.MaxNavSpeed)
{
DistancePID.Max = Mathf.Max(DistancePID.Min, max_speed);
}
}
//take into account vertical distance and obstacle
var rel_ahead = VSL.Altitude.Ahead - VSL.Altitude.Absolute;
// Log("vdist {}, rel.ahead {}, vF {}, aF {}", vdistance, rel_ahead,
// Utils.ClampL(1 - Mathf.Atan(vdistance/hdistance)/Utils.HalfPI, 0),
// Utils.ClampL(1 - rel_ahead/RAD.DistanceAhead, 0));//debug
vdistance = Mathf.Max(vdistance, rel_ahead);
if (vdistance > 0)
{
hdistance *= Utils.ClampL(1 - Mathf.Atan(vdistance / hdistance) / (float)Utils.HalfPI, 0);
}
if (RAD != null && rel_ahead > 0 && RAD.DistanceAhead > 0)
{
hdistance *= Utils.ClampL(1 - rel_ahead / RAD.DistanceAhead, 0);
}
//update the needed velocity
DistancePID.Update(hdistance);
var nV = vdir * DistancePID.Action;
//correcto for Follow Target program
if (CFG.Nav[Navigation.FollowTarget] && Vector3d.Dot(tvel, vdir) > 0)
{
nV += tvel;
}
VSL.HorizontalSpeed.SetNeeded(nV);
}
//correct for lateral movement
var latV = -Vector3d.Exclude(vdir, VSL.HorizontalSpeed.Vector);
var latF = (float)Math.Min((latV.magnitude / Math.Max(VSL.HorizontalSpeed.Absolute, 0.1)), 1);
LateralPID.P = C.LateralPID.P * latF;
LateralPID.I = Math.Min(C.LateralPID.I, latF);
LateralPID.D = C.LateralPID.D * latF;
LateralPID.Update(latV);
HSC.AddWeightedCorrection(LateralPID.Action);
// Log("\ndir v {}\nlat v {}\nact v {}\nlatPID {}",
// VSL.HorizontalSpeed.NeededVector, latV,
// LateralPID.Action, LateralPID);//debug
}
public bool Execute(Vector3d dV, float MinDeltaV = 0.1f, ManeuverCondition condition = null)
{
THR.Throttle = 0;
var has_correction = !course_correction.IsZero();
if (has_correction)
{
dV += course_correction;
course_correction = Vector3d.zero;
}
dVrem.Lower = MinDeltaV * 5;
dVrem.Upper = dVrem.Lower + 1;
dVrem.Value = dV.magnitude;
//prepare for the burn
VSL.Engines.ActivateEngines();
//orient along the burning vector
if (dVrem && VSL.Controls.RCSAvailableInDirection(-dV, (float)dVrem))
{
CFG.AT.OnIfNot(Attitude.KillRotation);
}
else
{
CFG.AT.OnIfNot(Attitude.Custom);
ATC.SetThrustDirW(-dV);
}
//check if need to be working
if (!working)
{
VSL.Engines.RequestClusterActivationForManeuver(dV);
working |= condition == null || condition((float)dVrem);
}
//end conditions for working execution
if (working && !has_correction)
{
//end if below the minimum dV
if (dVrem < MinDeltaV)
{
return(false);
}
//end if stalled
stalled.Update(dVrem, VSL.Controls.AttitudeError);
if (stalled)
{
return(false);
}
//prevent infinite dV tuning with inaccurate thrust-attitude systems
if (StopAtMinimum)
{
if (dVrem)
{
VSL.Controls.GimbalLimit = 0;
}
if (dVmin < 0)
{
dVmin = dVrem;
}
else if (dVrem || !ThrustWhenAligned || VSL.Controls.Aligned)
{
if (dVrem < dVmin)
{
dVmin = dVrem;
}
else if (dVrem - dVmin > MinDeltaV)
{
return(false);
}
}
}
}
//if not working and no correction, nothing left to do
if (!working && !has_correction)
{
return(true);
}
//use translation controls
if (VSL.Controls.TranslationAvailable &&
(dVrem || VSL.Controls.AttitudeError > AttitudeControlBase.C.AttitudeErrorThreshold))
{
TRA.AddDeltaV(-VSL.LocalDir(dV));
}
//use main throttle
if (ThrustWhenAligned)
{
THR.MaxThrottle = VSL.Controls.Aligned ? VSL.Engines.MaxThrottleForDeltaV(dV) : 0;
}
THR.CorrectThrottle = ThrustWhenAligned;
THR.DeltaV = (float)dVrem;
return(true);
}
protected override void OnAutopilotUpdate(FlightCtrlState s)
{
if (!IsActive)
{
return;
}
if (VSL.AutopilotDisabled)
{
filter.Reset(); return;
}
CFG.AT.OnIfNot(Attitude.Custom);
//calculate prerequisites
var thrust = VSL.Engines.DefThrust;
needed_thrust_dir = -VSL.Physics.Up;
if (!CFG.HF[HFlight.Level])
{
//set forward direction
if (CFG.HF[HFlight.NoseOnCourse] && !VSL.HorizontalSpeed.NeededVector.IsZero())
{
BRC.ForwardDirection = VSL.HorizontalSpeed.NeededVector;
}
//calculate horizontal velocity
CourseCorrection = Vector3d.zero;
for (int i = 0, count = CourseCorrections.Count; i < count; i++)
{
CourseCorrection += CourseCorrections[i];
}
var nV = VSL.HorizontalSpeed.NeededVector + CourseCorrection;
var hV = VSL.HorizontalSpeed.Vector - nV;
var hVl = VSL.LocalDir(hV);
var nVm = nV.magnitude;
var hVm = hV.magnitude;
var HVn = VSL.HorizontalSpeed.normalized;
//if the manual translation can and should be used
var rV = hV; //velocity that is needed to be handled by attitude control of the total thrust
var fV = Vector3d.zero; //forward-backward velocity with respect to the manual thrust vector
var with_manual_thrust = VSL.Engines.Active.Manual.Count > 0 &&
(nVm >= HSC.TranslationUpperThreshold ||
hVm >= HSC.TranslationUpperThreshold ||
CourseCorrection.magnitude >= HSC.TranslationUpperThreshold);
var manual_thrust = Vector3.ProjectOnPlane(VSL.Engines.DefManualThrust, VSL.Physics.Up);
var zero_manual_thrust = manual_thrust.IsZero();
if (with_manual_thrust &&
!zero_manual_thrust &&
Vector3.Dot(manual_thrust, hV) > 0)
{
thrust -= manual_thrust;
rV = Vector3.ProjectOnPlane(hV, manual_thrust);
fV = hV - rV;
}
var rVm = rV.magnitude;
var fVm = Utils.ClampL(fV.magnitude, 1e-5);
//calculate needed thrust direction
if (!(with_manual_thrust && zero_manual_thrust &&
VSL.HorizontalSpeed.Absolute <= HSC.TranslationLowerThreshold) &&
rVm > HSC.RotationLowerThreshold && Utils.ClampL(rVm / fVm, 0) > HSC.RotationLowerThreshold)
{
var GeeF = Mathf.Sqrt(VSL.Physics.G / Utils.G0);
var MaxHv = Utils.ClampL(Vector3d.Project(VSL.vessel.acceleration, rV).magnitude *HSC.AccelerationFactor, HSC.MinHvThreshold);
var upF = Utils.ClampL(Math.Pow(MaxHv / rVm, Utils.ClampL(HSC.HVCurve * GeeF, HSC.MinHVCurve)), GeeF) * Utils.ClampL(fVm / rVm, 1) / VSL.OnPlanetParams.TWRf;
// if(rVm > HSC.TranslationLowerThreshold) upF /= Utils.Clamp(VSL.Torque.MaxPitchRoll.AA_rad, 0.1f, 1);
needed_thrust_dir = rV.normalized - VSL.Physics.Up * upF;
}
//try to use translation controls (maneuver engines and RCS)
if (hVm > HSC.TranslationLowerThreshold && TRA != null && CFG.CorrectWithTranslation)
{
var nVn = nVm > 0? nV / nVm : Vector3d.zero;
var cV_lat = Vector3.ProjectOnPlane(CourseCorrection, nV);
if (nVm < HSC.TranslationUpperThreshold ||
Mathf.Abs((float)Vector3d.Dot(HVn, nVn)) < HSC.TranslationMaxCos)
{
TRA.AddDeltaV(hVl);
}
else if (cV_lat.magnitude > HSC.TranslationLowerThreshold)
{
TRA.AddDeltaV(-VSL.LocalDir(cV_lat));
}
}
//manual engine control
if (with_manual_thrust)
{
//turn the nose if nesessary
var pure_hV = VSL.HorizontalSpeed.Vector - VSL.HorizontalSpeed.NeededVector;
var NVm = VSL.HorizontalSpeed.NeededVector.magnitude;
var transF = 1f;
if (pure_hV.magnitude >= HSC.RotationUpperThreshold &&
(NVm < HSC.TranslationLowerThreshold ||
Vector3.Dot(HVn, VSL.HorizontalSpeed.NeededVector / NVm) < HSC.RotationMaxCos))
{
var max_MT = VSL.Engines.ManualThrustLimits.MaxInPlane(VSL.Physics.UpL);
if (!max_MT.IsZero())
{
var rot = Quaternion.AngleAxis(Vector3.Angle(max_MT, Vector3.ProjectOnPlane(VSL.OnPlanetParams.FwdL, VSL.Physics.UpL)),
VSL.Physics.Up * Mathf.Sign(Vector3.Dot(max_MT, Vector3.right)));
BRC.DirectionOverride = rot * pure_hV;
transF = Utils.ClampL(Vector3.Dot(VSL.OnPlanetParams.Fwd, BRC.DirectionOverride.normalized), 0);
transF *= transF * transF * transF;
}
}
translation_pid.I = (VSL.HorizontalSpeed > HSC.ManualTranslationIMinSpeed &&
VSL.vessel.mainBody.atmosphere)?
HSC.ManualTranslationPID.I * VSL.HorizontalSpeed : 0;
translation_pid.Update((float)fVm);
VSL.Controls.ManualTranslation = translation_pid.Action * hVl.CubeNorm() * transF;
// LogF("\nPID: {}\nManualTranslation: {}", translation_pid, VSL.Controls.ManualTranslation);
EnableManualTranslation(translation_pid.Action > 0);
}
else
{
EnableManualTranslation(false);
}
if (thrust.IsZero())
{
thrust = VSL.Engines.CurrentDefThrustDir;
}
if (CFG.HF[HFlight.Stop])
{
VSL.Altitude.DontCorrectIfSlow();
}
}
else
{
thrust = VSL.Engines.CurrentDefThrustDir;
VSL.Controls.ManualTranslationSwitch.Set(false);
}
needed_thrust_dir.Normalize();
//tune filter
filter.Tau = VSL.Engines.Slow ?
HSC.LowPassF / (1 + VSL.Torque.EnginesResponseTimeM * HSC.SlowTorqueF) :
HSC.LowPassF;
ATC.SetCustomRotationW(thrust, filter.Update(needed_thrust_dir).normalized);
#if DEBUG
// LogF("\nthrust {}\nneeded {}\nfilterred {}\nAttitudeError {}",
// thrust, needed_thrust_dir, filter.Value.normalized, VSL.Controls.AttitudeError);//debug
// CSV(VSL.Physics.UT,
// filter.Value.x, filter.Value.y, filter.Value.z,
// thrust.x, thrust.y, thrust.z);//debug
#endif
}
protected override void OnAutopilotUpdate()
{
var needed_thrust = -VSL.Physics.Up;
rotation_axis = Vector3.zero;
if (VSL.HasUserInput)
{
var angle = VTOL.MaxAngle * VSL.OnPlanetParams.TWRf;
var pitch_roll = Mathf.Abs(CS.pitch) + Mathf.Abs(CS.roll);
if (!CS.pitch.Equals(0))
{
needed_thrust = Quaternion.AngleAxis(-Mathf.Abs(CS.pitch) / pitch_roll * CS.pitch * angle, VSL.refT.right) * needed_thrust;
}
if (!CS.roll.Equals(0))
{
needed_thrust = Quaternion.AngleAxis(-Mathf.Abs(CS.roll) / pitch_roll * CS.roll * angle, VSL.Engines.refT_forward_axis) * needed_thrust;
}
compute_rotation(Rotation.Local(VSL.Engines.CurrentDefThrustDir, needed_thrust, VSL));
if (!CS.yaw.Equals(0))
{
rotation_axis = (rotation_axis * VSL.Controls.AttitudeError / angle - VSL.LocalDir(needed_thrust.normalized * CS.yaw * Mathf.PI / 3)).normalized;
VSL.Controls.SetAttitudeError(Mathf.Min(VSL.Controls.AttitudeError + Math.Abs(CS.yaw) * 30, 180));
}
compute_steering();
VSL.Controls.AddSteering(steering);
VSL.HasUserInput = false;
VSL.AutopilotDisabled = true;
CS.yaw = CS.pitch = CS.roll = 0;
}
else if (!(VSL.LandedOrSplashed || CFG.AT))
{
compute_rotation(Rotation.Local(VSL.Engines.CurrentDefThrustDir, needed_thrust, VSL));
compute_steering();
VSL.Controls.AddSteering(steering);
}
}
public bool Execute(Vector3d dV, float MinDeltaV = 0.1f, ManeuverCondition condition = null)
{
THR.Throttle = 0;
var has_correction = !course_correction.IsZero();
if (has_correction)
{
dV += course_correction;
course_correction = Vector3d.zero;
}
dVrem.Lower = MinDeltaV * 5;
dVrem.Upper = dVrem.Lower + 1;
dVrem.Value = dV.magnitude;
//end if below the minimum dV
if (dVrem < MinDeltaV)
{
return(false);
}
VSL.Engines.ActivateEngines();
//orient along the burning vector
if (dVrem && VSL.Controls.RCSAvailableInDirection(-dV))
{
CFG.AT.OnIfNot(Attitude.KillRotation);
}
else
{
CFG.AT.OnIfNot(Attitude.Custom);
ATC.SetThrustDirW(-dV);
}
if (!working)
{
VSL.Engines.RequestClusterActivationForManeuver(dV);
if (!has_correction && condition != null && !condition((float)dVrem))
{
return(true);
}
working = true;
}
//prevent infinite dV tuning with inaccurate thrust-attitude systems
if (StopAtMinimum)
{
if (dVrem)
{
VSL.Controls.GimbalLimit = 0;
}
if (dVmin < 0)
{
dVmin = dVrem;
ddV.Set(dVmin);
}
else if (!ThrustWhenAligned || VSL.Controls.Aligned)
{
ddV.Update(Math.Max(dVmin - dVrem, 0) / TimeWarp.fixedDeltaTime);
// Log("dVrem {}, dVmin {}, ddV {}, aliF {}, aliE {}",
// dVrem.Value, dVmin,
// ddV, VSL.Controls.AlignmentFactor, VSL.Controls.AttitudeError);//debug
if (ddV < MinDeltaV)
{
return(false);
}
if (dVrem < dVmin)
{
dVmin = dVrem;
}
else if (dVrem - dVmin > MinDeltaV)
{
return(false);
}
}
}
//use translation controls
if (VSL.Controls.TranslationAvailable)
{
if (dVrem || VSL.Controls.AttitudeError > GLB.ATCB.AttitudeErrorThreshold)
{
TRA.AddDeltaV(-VSL.LocalDir(dV));
}
if (dVrem && CFG.AT[Attitude.KillRotation])
{
var errorF = Utils.ClampL(Vector3.Dot(VSL.Engines.Thrust.normalized, -dV.normalized), 0);
THR.DeltaV = (float)dVrem * errorF * errorF;
return(true);
}
}
//use main throttle
if (ThrustWhenAligned)
{
THR.DeltaV = VSL.Controls.Aligned ? (float)dVrem * VSL.Controls.AlignmentFactor : 0;
}
else
{
THR.DeltaV = (float)dVrem;
}
return(true);
}
void compute_steering()
{
Vector3 v;
momentum_min.Update(VSL.vessel.angularMomentum.sqrMagnitude);
lthrust = VSL.LocalDir(VSL.Engines.CurrentDefThrustDir);
steering = Vector3.zero;
switch (CFG.AT.state)
{
case Attitude.Custom:
if (CustomRotation.Equals(default(Rotation)))
{
goto case Attitude.KillRotation;
}
lthrust = CustomRotation.current;
needed_lthrust = CustomRotation.needed;
break;
case Attitude.HoldAttitude:
if (refT != VSL.refT || !attitude_locked)
{
refT = VSL.refT;
locked_attitude = refT.rotation;
attitude_locked = true;
}
if (refT != null)
{
lthrust = Vector3.up;
needed_lthrust = refT.rotation.Inverse() * locked_attitude * lthrust;
}
break;
case Attitude.KillRotation:
if (refT != VSL.refT || momentum_min.True)
{
refT = VSL.refT;
locked_attitude = refT.rotation;
}
if (refT != null)
{
lthrust = Vector3.up;
needed_lthrust = refT.rotation.Inverse() * locked_attitude * lthrust;
}
break;
case Attitude.Prograde:
case Attitude.Retrograde:
v = VSL.InOrbit? VSL.vessel.obt_velocity : VSL.vessel.srf_velocity;
if (v.magnitude < GLB.THR.MinDeltaV)
{
CFG.AT.On(Attitude.KillRotation); break;
}
if (CFG.AT.state == Attitude.Prograde)
{
v *= -1;
}
needed_lthrust = VSL.LocalDir(v.normalized);
VSL.Engines.RequestNearestClusterActivation(needed_lthrust);
break;
case Attitude.RelVel:
case Attitude.AntiRelVel:
if (!VSL.HasTarget)
{
Message("No target");
CFG.AT.On(Attitude.KillRotation);
break;
}
v = VSL.InOrbit?
VSL.Target.GetObtVelocity() - VSL.vessel.obt_velocity :
VSL.Target.GetSrfVelocity() - VSL.vessel.srf_velocity;
if (v.magnitude < GLB.THR.MinDeltaV)
{
CFG.AT.On(Attitude.KillRotation); break;
}
if (CFG.AT.state == Attitude.AntiRelVel)
{
v *= -1;
}
needed_lthrust = VSL.LocalDir(v.normalized);
VSL.Engines.RequestClusterActivationForManeuver(v);
break;
case Attitude.ManeuverNode:
var solver = VSL.vessel.patchedConicSolver;
if (solver == null || solver.maneuverNodes.Count == 0)
{
Message("No maneuver node");
CFG.AT.On(Attitude.KillRotation);
break;
}
v = -solver.maneuverNodes[0].GetBurnVector(VSL.orbit);
needed_lthrust = VSL.LocalDir(v.normalized);
VSL.Engines.RequestClusterActivationForManeuver(v);
break;
case Attitude.Normal:
case Attitude.AntiNormal:
case Attitude.Radial:
case Attitude.AntiRadial:
case Attitude.Target:
case Attitude.AntiTarget:
VSL.Engines.RequestNearestClusterActivation(needed_lthrust);
break;
}
compute_steering(lthrust.normalized, needed_lthrust.normalized);
ResetCustomRotation();
}
public override void Update()
{
TWRf = 1;
CurrentThrustAccelerationTime = 0f;
CurrentThrustDecelerationTime = 0f;
update_aero_forces();
//calculate total downward thrust and slow engines' corrections
AeroForce = Lift + Drag;
var relAeroForce = AeroForce / (float)Utils.ClampL(VSL.vessel.dynamicPressurekPa, 1);
if (relAeroForce.sqrMagnitude > MaxAeroForceL.sqrMagnitude)
{
MaxAeroForceL = VSL.LocalDir(relAeroForce);
}
vLift = Vector3.Dot(AeroForce, VSL.Physics.Up);
MaxTWR = (VSL.Engines.MaxThrustM + Mathf.Max(vLift, 0)) / VSL.Physics.mg;
DTWR = Mathf.Max((vLift - Vector3.Dot(VSL.Engines.Thrust, VSL.Physics.Up)) / VSL.Physics.mg, 0);
DTWR_filter.TauUp = VSL.Engines.AccelerationTime90;
DTWR_filter.TauDown = VSL.Engines.DecelerationTime10;
DTWR_filter.Update(DTWR);
GeeVSF = 1 / Utils.ClampL(MaxTWR, 1);
var mVSFtor = (VSL.Torque.MaxPitchRoll.AA_rad > 0)?
Utils.ClampH(VerticalSpeedControl.C.MinVSFf / VSL.Torque.MaxPitchRoll.AA_rad, VerticalSpeedControl.C.MaxVSFtwr * GeeVSF) : 0;
MinVSF = Mathf.Lerp(0, mVSFtor, Mathf.Pow(VSL.Controls.Steering.sqrMagnitude, 0.25f));
var down_thrust = Mathf.Max(vLift, 0);
var slow_thrust = 0f;
var fast_thrust = 0f;
for (int i = 0; i < VSL.Engines.NumActive; i++)
{
var e = VSL.Engines.Active[i];
e.VSF = 1;
if (e.isVSC)
{
var dcomponent = -Vector3.Dot(e.wThrustDir, VSL.Physics.Up);
if (dcomponent <= 0)
{
e.VSF = VSL.HasUserInput? 0 : GeeVSF * VSL.Controls.InvAlignmentFactor;
}
else
{
var dthrust = e.nominalCurrentThrust(e.limit) * dcomponent;
if (e.useEngineResponseTime && dthrust > 0)
{
slow_thrust += dthrust;
CurrentThrustAccelerationTime += e.engineAccelerationSpeed * dthrust;
CurrentThrustDecelerationTime += e.engineDecelerationSpeed * dthrust;
}
else
{
fast_thrust = dthrust;
}
down_thrust += dthrust;
}
}
}
MaxDTWR = Utils.EWA(MaxDTWR, down_thrust / VSL.Physics.mg, 0.1f);
if (refT != null)
{
Fwd = Vector3.Cross(VSL.refT.right, -VSL.Engines.MaxThrust).normalized;
FwdL = refT.InverseTransformDirection(Fwd);
NoseUp = Mathf.Abs(Vector3.Dot(Fwd, VSL.refT.forward)) >= 0.9;
Heading = Vector3.ProjectOnPlane(Fwd, VSL.Physics.Up).normalized;
}
var controllable_thrust = slow_thrust + fast_thrust;
if (controllable_thrust > 0)
{
//correct setpoint for current TWR and slow engines
var rel_slow_thrust = slow_thrust * slow_thrust / controllable_thrust;
if (CurrentThrustAccelerationTime > 0)
{
CurrentThrustAccelerationTime = rel_slow_thrust / CurrentThrustAccelerationTime * VerticalSpeedControl.C.ASf;
}
if (CurrentThrustDecelerationTime > 0)
{
CurrentThrustDecelerationTime = rel_slow_thrust / CurrentThrustDecelerationTime * VerticalSpeedControl.C.DSf;
}
//TWR factor
var vsf = 1f;
if (VSL.VerticalSpeed.Absolute < 0)
{
vsf = Utils.Clamp(1 - (Utils.ClampH(CFG.VerticalCutoff, 0) - VSL.VerticalSpeed.Absolute) / ThrustDirectionControl.C.VSf, 1e-9f, 1);
}
var twr = VSL.Engines.Slow? DTWR_filter.Value : MaxTWR * Utils.Sin45; //MaxTWR at 45deg
TWRf = Utils.Clamp(twr / ThrustDirectionControl.C.TWRf, 1e-9f, 1) * vsf;
}
//parachutes
UnusedParachutes.Clear();
ActiveParachutes.Clear();
ParachutesActive = false;
ParachutesDeployed = false;
NearestParachuteStage = 0;
for (int i = 0, count = Parachutes.Count; i < count; i++)
{
var p = Parachutes[i];
if (!p.Valid)
{
continue;
}
p.Update();
if (p.Active)
{
ActiveParachutes.Add(p);
}
if (p.Usable)
{
UnusedParachutes.Add(p);
}
if (p.Stage > NearestParachuteStage)
{
NearestParachuteStage = p.Stage;
}
ParachutesDeployed |= p.Deployed;
}
ParachutesActive = ActiveParachutes.Count > 0;
HaveUsableParachutes = UnusedParachutes.Count > 0;
HaveParachutes = HaveUsableParachutes || ParachutesActive;
}
protected void compute_rotation()
{
Vector3 v;
momentum_min.Update(VSL.vessel.angularMomentum.sqrMagnitude);
lthrust = VSL.LocalDir(VSL.Engines.CurrentDefThrustDir);
steering = Vector3.zero;
switch (CFG.AT.state)
{
case Attitude.Custom:
if (!CustomRotation)
{
goto case Attitude.KillRotation;
}
lthrust = CustomRotation.current;
needed_lthrust = CustomRotation.needed;
VSL.Engines.RequestNearestClusterActivation(needed_lthrust);
break;
case Attitude.HoldAttitude:
if (refT != VSL.refT || !attitude_locked)
{
refT = VSL.refT;
locked_attitude = refT.rotation;
attitude_locked = true;
}
if (refT != null)
{
lthrust = Vector3.up;
needed_lthrust = refT.rotation.Inverse() * locked_attitude * lthrust;
}
break;
case Attitude.KillRotation:
if (refT != VSL.refT || momentum_min.True)
{
refT = VSL.refT;
locked_attitude = refT.rotation;
}
if (refT != null)
{
lthrust = Vector3.up;
needed_lthrust = refT.rotation.Inverse() * locked_attitude * lthrust;
}
break;
case Attitude.Prograde:
case Attitude.Retrograde:
v = VSL.InOrbit ? VSL.vessel.obt_velocity : VSL.vessel.srf_velocity;
if (v.magnitude < ThrottleControl.C.MinDeltaV)
{
CFG.AT.On(Attitude.KillRotation);
break;
}
if (CFG.AT.state == Attitude.Prograde)
{
v *= -1;
}
needed_lthrust = VSL.LocalDir(v.normalized);
VSL.Engines.RequestNearestClusterActivation(needed_lthrust);
break;
case Attitude.RelVel:
case Attitude.AntiRelVel:
if (!VSL.HasTarget)
{
Message("No target");
CFG.AT.On(Attitude.KillRotation);
break;
}
v = VSL.InOrbit ?
VSL.Target.GetObtVelocity() - VSL.vessel.obt_velocity :
VSL.Target.GetSrfVelocity() - VSL.vessel.srf_velocity;
if (v.magnitude < ThrottleControl.C.MinDeltaV)
{
CFG.AT.On(Attitude.KillRotation);
break;
}
if (CFG.AT.state == Attitude.AntiRelVel)
{
v *= -1;
}
needed_lthrust = VSL.LocalDir(v.normalized);
VSL.Engines.RequestClusterActivationForManeuver(-v);
break;
case Attitude.ManeuverNode:
var solver = VSL.vessel.patchedConicSolver;
if (solver == null || solver.maneuverNodes.Count == 0)
{
Message("No maneuver node");
CFG.AT.On(Attitude.KillRotation);
break;
}
v = -solver.maneuverNodes[0].GetBurnVector(VSL.orbit);
needed_lthrust = VSL.LocalDir(v.normalized);
VSL.Engines.RequestClusterActivationForManeuver(-v);
break;
case Attitude.Normal:
case Attitude.AntiNormal:
case Attitude.Radial:
case Attitude.AntiRadial:
case Attitude.Target:
case Attitude.AntiTarget:
VSL.Engines.RequestNearestClusterActivation(needed_lthrust);
break;
}
#if DEBUG
if (FollowMouse)
{
needed_lthrust = VSL.LocalDir(FlightCamera.fetch.mainCamera.ScreenPointToRay(Input.mousePosition).direction);
}
#endif
compute_rotation(lthrust.normalized, needed_lthrust.normalized);
ResetCustomRotation();
}
public float DistanceTo(EngineWrapper e)
{
return(DistanceTo(VSL.LocalDir(e.defThrustDir)));
}
protected override void OnAutopilotUpdate()
{
if (VSL.AutopilotDisabled)
{
output_filter.Reset(); return;
}
CFG.AT.OnIfNot(Attitude.Custom);
//calculate prerequisites
var thrust = VSL.Engines.DefThrust;
needed_thrust_dir = -VSL.Physics.Up;
if (CFG.HF[HFlight.Level])
{
thrust = VSL.Engines.CurrentDefThrustDir;
VSL.Controls.ManualTranslationSwitch.Set(false);
}
else
{
//set forward direction
if (CFG.HF[HFlight.NoseOnCourse] && !VSL.HorizontalSpeed.NeededVector.IsZero())
{
BRC.ForwardDirection = VSL.HorizontalSpeed.NeededVector;
}
//calculate horizontal velocity
CourseCorrection = Vector3d.zero;
for (int i = 0, count = CourseCorrections.Count; i < count; i++)
{
CourseCorrection += CourseCorrections[i];
}
var needed_vector = VSL.HorizontalSpeed.NeededVector + CourseCorrection;
var error_vector = VSL.HorizontalSpeed.Vector - needed_vector;
var error_vector_local = VSL.LocalDir(error_vector);
var needed_abs = needed_vector.magnitude;
var error_abs = error_vector.magnitude;
var horizontal_speed_dir = VSL.HorizontalSpeed.normalized;
var rotation_vector = error_vector; //velocity that is needed to be handled by attitude control of the total thrust
var translaion_vector = Vector3d.zero; //forward-backward velocity with respect to the manual thrust vector
var rotation_abs = error_abs;
var translation_abs = 0.0;
//decide if manual thrust can and should be used
var with_manual_thrust = VSL.Engines.Active.Manual.Count > 0 &&
(needed_abs >= C.TranslationMaxDeltaV ||
error_abs >= C.TranslationMaxDeltaV ||
CourseCorrection.magnitude >= C.TranslationMaxDeltaV);
manual_thrust = Vector3.zero;
if (with_manual_thrust)
{
var forward_dir = VSL.HorizontalSpeed.NeededVector.IsZero()?
VSL.OnPlanetParams.Fwd : (Vector3)VSL.HorizontalSpeed.NeededVector;
//first, see if we need to turn the nose so that the maximum manual thrust points the right way
var translation_factor = 1f;
var pure_error_vector = VSL.HorizontalSpeed.Vector - VSL.HorizontalSpeed.NeededVector;
var pure_needed_abs = VSL.HorizontalSpeed.NeededVector.magnitude;
if (pure_error_vector.magnitude >= C.ManualThrust.Turn_MinDeltaV &&
(pure_needed_abs < C.TranslationMinDeltaV ||
Vector3.ProjectOnPlane(VSL.HorizontalSpeed.NeededVector, horizontal_speed_dir)
.magnitude > C.ManualThrust.Turn_MinLateralDeltaV))
{
manual_thrust = VSL.Engines.ManualThrustLimits.MaxInPlane(VSL.Physics.UpL);
if (!manual_thrust.IsZero())
{
var fwdH = Vector3.ProjectOnPlane(VSL.OnPlanetParams.FwdL, VSL.Physics.UpL);
var angle = Utils.Angle2(manual_thrust, fwdH);
var rot = Quaternion.AngleAxis(angle, VSL.Physics.Up * Mathf.Sign(Vector3.Dot(manual_thrust, Vector3.right)));
BRC.DirectionOverride = rot * pure_error_vector;
translation_factor = Utils.ClampL((Vector3.Dot(VSL.OnPlanetParams.Fwd, BRC.DirectionOverride.normalized) - 0.5f), 0) * 2;
forward_dir = BRC.DirectionOverride;
}
}
//simply use manual thrust currently available in the forward direction
else if (Vector3.Dot(forward_dir, error_vector) < 0)
{
manual_thrust = VSL.Engines.ManualThrustLimits.Slice(VSL.LocalDir(-forward_dir));
translation_factor = Utils.ClampL((Vector3.Dot(VSL.WorldDir(manual_thrust.normalized), -forward_dir.normalized) - 0.5f), 0) * 2;
}
with_manual_thrust = !manual_thrust.IsZero();
if (with_manual_thrust)
{
thrust = VSL.Engines.CurrentDefThrustDir;
rotation_vector = Vector3.ProjectOnPlane(error_vector, forward_dir);
translaion_vector = error_vector - rotation_vector;
rotation_abs = rotation_vector.magnitude;
translation_abs = Utils.ClampL(translaion_vector.magnitude, 1e-5);
translation_factor *= Utils.Clamp(1 + Vector3.Dot(thrust.normalized, pure_error_vector.normalized) * C.ManualThrust.ThrustF, 0, 1);
translation_factor *= translation_factor * translation_factor * translation_factor;
translation_pid.I = (VSL.HorizontalSpeed > C.ManualThrust.I_MinSpeed &&
VSL.vessel.mainBody.atmosphere)?
C.ManualThrust.PID.I * VSL.HorizontalSpeed : 0;
var D = VSL.Engines.ManualThrustSpeed.Project(error_vector_local.normalized).magnitude;
if (D > 0)
{
D = Mathf.Min(C.ManualThrust.PID.D / D, C.ManualThrust.D_Max);
}
translation_pid.D = D;
translation_pid.Update((float)translation_abs);
VSL.Controls.ManualTranslation = translation_pid.Action * error_vector_local.CubeNorm() * translation_factor;
EnableManualThrust(translation_pid.Action > 0);
}
}
if (!with_manual_thrust)
{
EnableManualThrust(false);
}
//use attitude control to point total thrust to modify horizontal velocity
if (rotation_abs > C.RotationMinDeltaV &&
Utils.ClampL(rotation_abs / translation_abs, 0) > C.RotationMinDeltaV &&
(!with_manual_thrust ||
VSL.HorizontalSpeed.Absolute > C.TranslationMinDeltaV))
{
var GeeF = Mathf.Sqrt(VSL.Physics.G / Utils.G0);
var MaxHv = Utils.ClampL(Vector3d.Project(VSL.vessel.acceleration, rotation_vector).magnitude *C.AccelerationFactor, C.MinHvThreshold);
var upF = Utils.ClampL(Math.Pow(MaxHv / rotation_abs, Utils.ClampL(C.HVCurve * GeeF, C.MinHVCurve)), GeeF) *
Utils.ClampL(translation_abs / rotation_abs, 1) / VSL.OnPlanetParams.TWRf;
needed_thrust_dir = rotation_vector.normalized - VSL.Physics.Up * upF;
}
//try to use translation controls (maneuver engines and RCS)
if (error_abs > C.TranslationMinDeltaV && TRA != null && CFG.CorrectWithTranslation)
{
var nVn = needed_abs > 0? needed_vector / needed_abs : Vector3d.zero;
var cV_lat = Vector3.ProjectOnPlane(CourseCorrection, needed_vector);
if (needed_abs < C.TranslationMaxDeltaV ||
Mathf.Abs((float)Vector3d.Dot(horizontal_speed_dir, nVn)) < C.TranslationMaxCos)
{
TRA.AddDeltaV(error_vector_local);
}
else if (cV_lat.magnitude > C.TranslationMinDeltaV)
{
TRA.AddDeltaV(-VSL.LocalDir(cV_lat));
}
}
//sanity check
if (thrust.IsZero())
{
thrust = VSL.Engines.CurrentDefThrustDir;
}
//no need to avoid static obstacles if we're stopped
if (CFG.HF[HFlight.Stop])
{
VSL.Altitude.DontCorrectIfSlow();
}
}
needed_thrust_dir.Normalize();
//tune filter
output_filter.Tau = VSL.Torque.Slow ?
C.LowPassF / (1 + VSL.Torque.EnginesResponseTimeM * C.SlowTorqueF) :
C.LowPassF;
ATC.SetCustomRotationW(thrust, output_filter.Update(needed_thrust_dir).normalized);
#if DEBUG
// LogF("\nthrust {}\nneeded {}\nfilterred {}\nAttitudeError {}",
// thrust, needed_thrust_dir, filter.Value.normalized, VSL.Controls.AttitudeError);//debug
// CSV(VSL.Physics.UT,
// filter.Value.x, filter.Value.y, filter.Value.z,
// thrust.x, thrust.y, thrust.z);//debug
#endif
}