// ThrottleControl THR;
//
// OscillationDetector3D OD = new OscillationDetector3D(3, 30, 100, 200, 1);
// Timer test_timer = new Timer(1);
// static readonly Vector3[] axes = {Vector3.up, Vector3.right};
// Vector3 needed_thrust;
// int direction = 1, axis = 0;
// float throttle = 0.1f, dThrottle = 0.1f;
// float curAAf = 0.1f, dAAf = 0.05f;
// enum TestStage {START, TESTING, KILL_ROT, NEXT_AAf, NEXT_THROTTLE, NEXT_AXIS, DONE};
// TestStage stage = TestStage.START;
//
// void tune_steering_test(Vector3 AAf)
// {
// VSL.Controls.GimbalLimit = 0;//VSL.OnPlanetParams.TWRf*100;
// //tune PID parameters
// var angularV = VSL.vessel.angularVelocity;
// var angularM = Vector3.Scale(angularV, VSL.Physics.MoI);
// var slow = VSL.Engines.Slow?
// (Vector3.one+Vector3.Scale(VSL.Torque.EnginesResponseTime,
// VSL.Torque.Engines.SpecificTorque)*ATCB.SlowTorqueF)
// .ClampComponentsH(ATCB.MaxSlowF) : Vector3.one;
// var slowi = slow.Inverse();
// var iErr = (Vector3.one-angle_error);
// var PIf = AAf.ScaleChain(iErr.ClampComponentsL(1/ATCB.MaxEf)*ATCB.MaxEf, slowi);
//// var AA_clamped = AA.ClampComponentsH(ATCB.MaxAA);
// steering_pid.P = Vector3.Scale(ATCB.PID.P, PIf);
// steering_pid.I = Vector3.Scale(ATCB.PID.I, PIf);
// steering_pid.D = ATCB.PID.D.ScaleChain((iErr
//// + (Vector3.one-AA_clamped/ATCB.MaxAA)
// + angularM.AbsComponents()*ATCB.AngularMf
// ).ClampComponentsH(1),
//
// AAf, slow,slow).ClampComponentsL(0);
// //add inertia to handle constantly changing needed direction
// var inertia = angularM.Sign()
// .ScaleChain(iErr,
// angularM, angularM,
// Vector3.Scale(VSL.Torque.MaxCurrent.Torque, VSL.Physics.MoI).Inverse(0))
// .ClampComponents(-Mathf.PI, Mathf.PI)
// /Mathf.Lerp(ATCB.InertiaFactor, 1, VSL.Physics.MoI.magnitude*ATCB.MoIFactor);
// steering += inertia;
// //update PID
// steering_pid.Update(steering, angularV);
// steering = Vector3.Scale(steering_pid.Action, slowi);
// //postprocessing by derived classes
// correct_steering();
// }
//
//
// protected override void OnAutopilotUpdate(FlightCtrlState s)
// {
// if(!(CFG.Enabled && stage != TestStage.DONE && VSL.refT != null && VSL.orbit != null)) return;
// if(VSL.AutopilotDisabled) { reset(); return; }
// lthrust = VSL.LocalDir(VSL.Engines.CurrentDefThrustDir).normalized;
// if(VSL.IsActiveVessel)
// TCAGui.DebugMessage = Utils.Format("stage: {}, axis: {}\ntimer: {}\ncur AAf {}, throttle {}\n",
// stage, axis, test_timer, curAAf, throttle);
// switch(stage)
// {
// case TestStage.START:
// CheatOptions.InfinitePropellant = true;
// CheatOptions.InfiniteElectricity = true;
// CheatOptions.IgnoreMaxTemperature = true;
// VSL.vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, true);
// Debug.ClearDeveloperConsole();
// needed_thrust = VSL.WorldDir(Quaternion.AngleAxis(120*direction, axes[axis])*lthrust);
// OD.Reset();
// test_timer.Reset();
// direction = -direction;
// stage = TestStage.TESTING;
// break;
// case TestStage.KILL_ROT:
// THR.Throttle = 0;
// CFG.AT.Off();
// VSL.vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true);
// if(VSL.vessel.angularVelocity.sqrMagnitude > 1e-4) break;
// VSL.vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, false);
// stage = TestStage.START;
// break;
// case TestStage.TESTING:
// CFG.AT.OnIfNot(Attitude.Custom);
// THR.Throttle = throttle;
// needed_lthrust = VSL.LocalDir(needed_thrust);
// compute_steering(lthrust, needed_lthrust);
// tune_steering_test(Vector3.one*curAAf);
// VSL.Controls.AddSteering(steering);
// //detect oscillations
// OD.Update(steering, TimeWarp.fixedDeltaTime);
// if(VSL.IsActiveVessel)
// TCAGui.DebugMessage +=
// string.Format("pid: {0}\nsteering: {1}%\ngimbal limit: {2}\nOD: {3}",
// steering_pid, steering_pid.Action*100, VSL.Controls.GimbalLimit, OD.Value);
//
// if(OD.Value.x > 0.1 ||
// OD.Value.y > 0.1 ||
// OD.Value.z > 0.1)
// stage = TestStage.NEXT_THROTTLE;
// else if(VSL.Controls.AttitudeError < 1)
// stage = TestStage.NEXT_AAf;
// break;
// case TestStage.NEXT_AAf:
// curAAf += dAAf;
// stage = curAAf > 5+dAAf/2 ? TestStage.NEXT_THROTTLE : TestStage.KILL_ROT;
// break;
// case TestStage.NEXT_THROTTLE:
// CSV(axes[axis] == Vector3.up? 0 : 1, throttle, AA, OD.Value, steering_pid.D, curAAf-dAAf);
// curAAf = 0.1f;
// throttle += dThrottle;
// if(throttle > 1 && throttle < 1+dThrottle/2)
// throttle = 1;
// stage = throttle > 1 ? TestStage.NEXT_AXIS : TestStage.KILL_ROT;
// break;
// case TestStage.NEXT_AXIS:
// throttle = 0.1f;
// axis += 1;
// stage = axis < axes.Length ? TestStage.KILL_ROT : TestStage.DONE;
// break;
// }
// }
public void DrawDebugLines()
{
if (!CFG.AT || VSL == null || VSL.vessel == null || VSL.refT == null)
{
return;
}
// Utils.GLVec(VSL.refT.position, VSL.OnPlanetParams.Heading.normalized*2500, Color.white);
Utils.GLVec(VSL.refT.position, VSL.WorldDir(lthrust.normalized) * 20, Color.yellow);
Utils.GLVec(VSL.refT.position, VSL.WorldDir(needed_lthrust.normalized) * 20, Color.red);
Utils.GLVec(VSL.refT.position, VSL.WorldDir(VSL.vessel.angularVelocity * 20), Color.cyan);
Utils.GLVec(VSL.refT.position, VSL.WorldDir(new Vector3(pid_pitch.atPID.Action * rotation_axis.x,
pid_roll.atPID.Action * rotation_axis.y,
pid_yaw.atPID.Action * rotation_axis.z) * 20), Color.green);
// Utils.GLVec(VSL.refT.position, VSL.WorldDir(steering*20), Color.cyan);
// Utils.GLVec(VSL.refT.position, VSL.WorldDir(steering_pid.Action*20), Color.magenta);
// Utils.GLVec(VSL.refT.position, VSL.refT.right*2, Color.red);
// Utils.GLVec(VSL.refT.position, VSL.refT.forward*2, Color.blue);
// Utils.GLVec(VSL.refT.position, VSL.refT.up*2, Color.green);
// if(VSL.Target != null)
// Utils.GLDrawPoint(VSL.Target.GetTransform().position, Color.red, 5);
//
// VSL.Engines.All.ForEach(e =>
// {
// Utils.GLVec(e.wThrustPos, e.wThrustDir*2, Color.red);
// Utils.GLVec(e.wThrustPos, e.defThrustDir*2, Color.yellow);
// });
}
public void DrawDebugLines()
{
if (VSL == null || VSL.vessel == null || VSL.refT == null || !CFG.HF)
{
return;
}
Utils.GLVec(VSL.refT.position, VSL.HorizontalSpeed.NeededVector, Color.yellow);
Utils.GLVec(VSL.refT.position, VSL.WorldDir(manual_thrust), Color.magenta);
Utils.GLVec(VSL.refT.position + VSL.Physics.Up * VSL.Geometry.H, VSL.HorizontalSpeed.Vector, Color.red);
Utils.GLVec(VSL.refT.position + VSL.Physics.Up * VSL.Geometry.H * 1.1, CourseCorrection, Color.green);
}
protected override void DrawContent()
{
#if DEBUG
ATC.DrawDebugLines();
Utils.GLVec(VSL.refT.position, VSL.WorldDir(VSL.OnPlanetParams.MaxAeroForceL) * 10, Color.magenta);
#endif
if (GUILayout.Button(new GUIContent("T-SAS", "Push to toggle attitude controls"),
CFG.AT && !VSL.AutopilotDisabled? Styles.cyan : Styles.white, GUILayout.ExpandWidth(false)))
{
cues.Toggle();
}
}
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 DrawDebugLines()
{
if (VSL == null || VSL.vessel == null || VSL.refT == null)
{
return;
}
// Utils.GLVec(VSL.refT.position, VSL.OnPlanetParams.Heading.normalized*2500, Color.white);
Utils.GLVec(VSL.refT.position, VSL.Engines.CurrentThrustDir * 20, Color.cyan);
Utils.GLVec(VSL.refT.position, VSL.WorldDir(needed_lthrust.normalized) * 20, Color.green);
// Utils.GLVec(VSL.refT.position, VSL.WorldDir(VSL.vessel.angularVelocity*20), Color.green);
// Utils.GLVec(VSL.refT.position, VSL.WorldDir(steering*20), Color.cyan);
// Utils.GLVec(VSL.refT.position, VSL.WorldDir(steering_pid.Action*20), Color.magenta);
// Utils.GLVec(VSL.refT.position, VSL.refT.right*2, Color.red);
// Utils.GLVec(VSL.refT.position, VSL.refT.forward*2, Color.blue);
// Utils.GLVec(VSL.refT.position, VSL.refT.up*2, Color.green);
}
public override void Draw()
{
base.Draw();
if (!IsActive || CFG.CTRL.Not(ControlMode.VTOL))
{
return;
}
Utils.GLVec(VSL.refT.position, VSL.Engines.MaxThrust.normalized * 20, Color.yellow);
Utils.GLVec(VSL.refT.position, needed_thrust.normalized * 20, Color.red);
Utils.GLVec(VSL.refT.position, VSL.WorldDir(VSL.vessel.angularVelocity * 20), Color.cyan);
Utils.GLVec(VSL.refT.position, VSL.WorldDir(rotation_axis * 25), Color.green);
// if(!steering.IsZero())
// Utils.GLVec(VSL.Physics.wCoM, VSL.WorldDir(steering.normalized*20), Color.cyan);
// Utils.GLVec(VSL.Physics.wCoM, VSL.refT.up*3, Color.green);
// Utils.GLVec(VSL.Physics.wCoM, VSL.refT.forward*3, Color.blue);
// Utils.GLVec(VSL.Physics.wCoM, VSL.refT.right*3, Color.red);
}
public override void Draw()
{
base.Draw();
if (!IsActive || CFG.CTRL.Not(ControlMode.VTOL))
{
return;
}
if (!VSL.Engines.MaxThrust.IsZero())
{
Utils.GLVec(VSL.Physics.wCoM, VSL.Engines.MaxThrust.normalized * 20, Color.red);
}
if (!needed_thrust.IsZero())
{
Utils.GLVec(VSL.Physics.wCoM, needed_thrust.normalized * 20, Color.yellow);
}
if (!steering.IsZero())
{
Utils.GLVec(VSL.Physics.wCoM, VSL.WorldDir(steering.normalized * 20), Color.cyan);
}
Utils.GLVec(VSL.Physics.wCoM, VSL.refT.up * 3, Color.green);
Utils.GLVec(VSL.Physics.wCoM, VSL.refT.forward * 3, Color.blue);
Utils.GLVec(VSL.Physics.wCoM, VSL.refT.right * 3, Color.red);
}
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
}