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 }