diff --git a/MechJeb2/AttitudeControllers/BetterController.cs b/MechJeb2/AttitudeControllers/BetterController.cs index 96d9410dd..76cf92fdb 100644 --- a/MechJeb2/AttitudeControllers/BetterController.cs +++ b/MechJeb2/AttitudeControllers/BetterController.cs @@ -214,7 +214,7 @@ private void UpdatePredictionPI() // lowpass filter on the error input _error0 = _error1.IsFinite() ? _error1 + PosSmoothIn * (_error0 - _error1) : _error0; - Vector3d controlTorque = ac.torque; + Vector3d controlTorque = ac.torque.SelectAxis(_error0); // needed to stop wiggling at higher phys warp double warpFactor = Math.Pow(ac.vesselState.deltaT / 0.02, 0.90); // the power law here comes ultimately from the simulink PID tuning app @@ -277,7 +277,7 @@ private void UpdatePredictionPI() if (Math.Abs(_actuation[i]) < EPS || double.IsNaN(_actuation[i])) _actuation[i] = 0; - _targetTorque[i] = _actuation[i] / ac.torque[i]; + _targetTorque[i] = _actuation[i] / controlTorque[i]; if (ac.ActuationControl[i] == 0) Reset(i); diff --git a/MechJeb2/AttitudeControllers/HybridController.cs b/MechJeb2/AttitudeControllers/HybridController.cs index 9d3b04b41..0c62fc164 100644 --- a/MechJeb2/AttitudeControllers/HybridController.cs +++ b/MechJeb2/AttitudeControllers/HybridController.cs @@ -39,7 +39,7 @@ class HybridController : BaseAttitudeController /* max angular rotation */ private Vector3d MaxOmega = Vector3d.zero; - private Vector3d ControlTorque { get { return ac.torque; } } + private Vector3d ControlTorque { get { return ac.torque.ToVector3d(); } } public HybridController(MechJebModuleAttitudeController controller) : base(controller) { diff --git a/MechJeb2/AttitudeControllers/KosAttitudeController.cs b/MechJeb2/AttitudeControllers/KosAttitudeController.cs index 0992e6520..87219139a 100644 --- a/MechJeb2/AttitudeControllers/KosAttitudeController.cs +++ b/MechJeb2/AttitudeControllers/KosAttitudeController.cs @@ -22,7 +22,7 @@ class KosAttitudeController : BaseAttitudeController public KosPIDLoop pitchRatePI = new KosPIDLoop(1, 0.1, 0, extraUnwind: true); public KosPIDLoop yawRatePI = new KosPIDLoop(1, 0.1, 0, extraUnwind: true); public KosPIDLoop rollRatePI = new KosPIDLoop(1, 0.1, 0, extraUnwind: true); - + private Vector3d Actuation = Vector3d.zero; private Vector3d TargetTorque = Vector3d.zero; private Vector3d Omega = Vector3d.zero; @@ -36,7 +36,7 @@ class KosAttitudeController : BaseAttitudeController /* max angular rotation */ private Vector3d MaxOmega = Vector3d.zero; - private Vector3d ControlTorque { get { return ac.torque; } } + private Vector3d ControlTorque { get { return ac.torque.ToVector3d(); } } public KosAttitudeController(MechJebModuleAttitudeController controller) : base(controller) { @@ -52,7 +52,7 @@ public override void DrivePre(FlightCtrlState s, out Vector3d act, out Vector3d deltaEuler = phiVector * Mathf.Rad2Deg; act = Actuation; } - + /* temporary state vectors */ private Quaternion vesselRotation; private Vector3d vesselForward; @@ -60,7 +60,7 @@ public override void DrivePre(FlightCtrlState s, out Vector3d act, out Vector3d private Vector3d vesselStarboard; private Vector3d targetForward; private Vector3d targetTop; - + /* private Vector3d targetStarboard; */ private void UpdateStateVectors() { @@ -102,12 +102,12 @@ public Vector3d PhiVector() { return Phi; } - + private void UpdatePredictionPI() { phiTotal = PhiTotal(); phiVector = PhiVector(); - + for(int i = 0; i < 3; i++) { MaxOmega[i] = ControlTorque[i] * maxStoppingTime / ac.vesselState.MoI[i]; } @@ -135,7 +135,7 @@ public override void Reset() yawRatePI.ResetI(); rollRatePI.ResetI(); } - + private void UpdateControl() { /* TODO: static engine torque and/or differential throttle */ diff --git a/MechJeb2/AttitudeControllers/MJAttitudeController.cs b/MechJeb2/AttitudeControllers/MJAttitudeController.cs index 7dfa9540a..6d223e6b8 100644 --- a/MechJeb2/AttitudeControllers/MJAttitudeController.cs +++ b/MechJeb2/AttitudeControllers/MJAttitudeController.cs @@ -190,7 +190,7 @@ public override void DrivePre(FlightCtrlState s, out Vector3d act, out Vector3d Math.Max(-Math.PI, Math.Min(Math.PI, err.z))); // ( MoI / available torque ) factor: - Vector3d NormFactor = Vector3d.Scale(ac.vesselState.MoI, ac.torque.InvertNoNaN()); + Vector3d NormFactor = Vector3d.Scale(ac.vesselState.MoI, ac.torque.ToVector3d().InvertNoNaN()); err.Scale(NormFactor); @@ -202,7 +202,7 @@ public override void DrivePre(FlightCtrlState s, out Vector3d act, out Vector3d omega.Scale(NormFactor); if (Tf_autoTune) - tuneTf(ac.torque); + tuneTf(ac.torque.ToVector3d()); setPIDParameters(); // angular velocity limit: diff --git a/MechJeb2/MechJebModuleAttitudeAdjustment.cs b/MechJeb2/MechJebModuleAttitudeAdjustment.cs index 60837e02b..e33f8ea80 100644 --- a/MechJeb2/MechJebModuleAttitudeAdjustment.cs +++ b/MechJeb2/MechJebModuleAttitudeAdjustment.cs @@ -60,7 +60,7 @@ protected override void WindowGUI(int windowID) GUILayout.BeginHorizontal(); GUILayout.Label(Localizer.Format("#MechJeb_AttitudeAdjust_Label2"), GUILayout.ExpandWidth(true));//Torque - GUILayout.Label("|" + core.attitude.torque.magnitude.ToString("F3") + "| " + MuUtils.PrettyPrint(core.attitude.torque), + GUILayout.Label("|" + core.attitude.torque.ToVector3d().magnitude.ToString("F3") + "| " + MuUtils.PrettyPrint(core.attitude.torque), GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); @@ -71,7 +71,7 @@ protected override void WindowGUI(int windowID) GUILayout.ExpandWidth(false)); GUILayout.EndHorizontal(); - Vector3d ratio = Vector3d.Scale(vesselState.MoI, core.attitude.torque.InvertNoNaN()); + Vector3d ratio = Vector3d.Scale(vesselState.MoI, core.attitude.torque.ToVector3d().InvertNoNaN()); GUILayout.BeginHorizontal(); GUILayout.Label(Localizer.Format("#MechJeb_AttitudeAdjust_Label4"), GUILayout.ExpandWidth(true));//MOI / torque diff --git a/MechJeb2/MechJebModuleAttitudeController.cs b/MechJeb2/MechJebModuleAttitudeController.cs index 99ba9aea2..341d9e01c 100644 --- a/MechJeb2/MechJebModuleAttitudeController.cs +++ b/MechJeb2/MechJebModuleAttitudeController.cs @@ -106,7 +106,7 @@ private set public double attitudeError; - public Vector3d torque; + public Vector6 torque; public Vector3d inertia; public MechJebModuleAttitudeController(MechJebCore core) @@ -322,14 +322,14 @@ public override void OnFixedUpdate() torque = vesselState.torqueAvailable; if (core.thrust.differentialThrottle && core.thrust.differentialThrottleSuccess == MechJebModuleThrustController.DifferentialThrottleStatus.Success) - torque += vesselState.torqueDiffThrottle * vessel.ctrlState.mainThrottle / 2.0; + torque.positive += vesselState.torqueDiffThrottle * vessel.ctrlState.mainThrottle / 2.0; // Inertia is a bad name. It's the "angular distance to stop" inertia = 0.5 * Vector3d.Scale( vesselState.angularMomentum.Sign(), Vector3d.Scale( Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum), - Vector3d.Scale(torque, vesselState.MoI).InvertNoNaN() + Vector3d.Scale(torque.ToVector3d(), vesselState.MoI).InvertNoNaN() ) ); Controller.OnFixedUpdate(); diff --git a/MechJeb2/MechJebModuleRoverController.cs b/MechJeb2/MechJebModuleRoverController.cs index 49ab59177..414232265 100644 --- a/MechJeb2/MechJebModuleRoverController.cs +++ b/MechJeb2/MechJebModuleRoverController.cs @@ -326,7 +326,7 @@ public override void Drive(FlightCtrlState s) // TODO put the brake in when runn Vector3.OrthoNormalize(ref norm, ref fwd); Quaternion quat = Quaternion.LookRotation(fwd, norm); - if (vesselState.torqueAvailable.sqrMagnitude > 0) + if (vesselState.torqueAvailable.ToVector3d().sqrMagnitude > 0) core.attitude.attitudeTo(quat, AttitudeReference.INERTIAL, this); } diff --git a/MechJeb2/MechJebModuleThrustController.cs b/MechJeb2/MechJebModuleThrustController.cs index 7c4bbd4d9..91bb084b9 100644 --- a/MechJeb2/MechJebModuleThrustController.cs +++ b/MechJeb2/MechJebModuleThrustController.cs @@ -392,11 +392,11 @@ public override void Drive(FlightCtrlState s) } else { - bool useGimbal = (vesselState.torqueGimbal.positive.x > vesselState.torqueAvailable.x * 10) || - (vesselState.torqueGimbal.positive.z > vesselState.torqueAvailable.z * 10); + bool useGimbal = (vesselState.torqueGimbal.positive.x > vesselState.torqueAvailable.positive.x * 10) || + (vesselState.torqueGimbal.positive.z > vesselState.torqueAvailable.positive.z * 10); - bool useDiffThrottle = (vesselState.torqueDiffThrottle.x > vesselState.torqueAvailable.x * 10) || - (vesselState.torqueDiffThrottle.z > vesselState.torqueAvailable.z * 10); + bool useDiffThrottle = (vesselState.torqueDiffThrottle.x > vesselState.torqueAvailable.positive.x * 10) || + (vesselState.torqueDiffThrottle.z > vesselState.torqueAvailable.positive.z * 10); if ((core.attitude.attitudeError >= 2) && (useGimbal || (useDiffThrottle && core.thrust.differentialThrottle))) { diff --git a/MechJeb2/MuUtils.cs b/MechJeb2/MuUtils.cs index 3797102ea..ee74eab88 100644 --- a/MechJeb2/MuUtils.cs +++ b/MechJeb2/MuUtils.cs @@ -65,6 +65,12 @@ public static string PrettyPrint(Vector3d vector, string format = "F3") return "[" + PadPositive(vector.x, format) + ", " + PadPositive(vector.y, format) + ", " + PadPositive(vector.z, format) + " ]"; } + public static string PrettyPrint(Vector6 vector, string format = "F3") + { + return "[" + PadPositive(vector.positive.x,format) + ", " + PadPositive(vector.positive.y,format) + ", " + PadPositive(vector.positive.z,format) + + " ], [" + PadPositive(-vector.negative.x,format) + ", " + PadPositive(-vector.negative.y,format) + ", " + PadPositive(-vector.negative.z,format) + " ]"; + } + public static string PrettyPrint(Quaternion quaternion, string format = "F3") { return "[" + PadPositive(quaternion.x, format) + ", " + PadPositive(quaternion.y, format) + ", " + PadPositive(quaternion.z, format) + ", " + PadPositive(quaternion.w ,format) + "]"; @@ -171,7 +177,7 @@ public static bool PhysicsRunning() { return (TimeWarp.WarpMode == TimeWarp.Modes.LOW) || (TimeWarp.CurrentRateIndex == 0); } - + public static string SystemClipboard { get => GUIUtility.systemCopyBuffer; diff --git a/MechJeb2/Vector6.cs b/MechJeb2/Vector6.cs index c5be85b83..4ba14de6b 100644 --- a/MechJeb2/Vector6.cs +++ b/MechJeb2/Vector6.cs @@ -2,9 +2,9 @@ namespace MuMech { - public class Vector6 : IConfigNode + public struct Vector6 : IConfigNode { - public Vector3d positive = Vector3d.zero, negative = Vector3d.zero; + public Vector3d positive, negative; public enum Direction { FORWARD=0, BACK=1, UP=2, DOWN=3, RIGHT=4, LEFT=5 }; @@ -66,7 +66,6 @@ public double this[Direction index] } } - public Vector6() { } public Vector6(Vector3d positive, Vector3d negative) { this.positive = positive; @@ -107,6 +106,21 @@ public double GetMagnitude(Vector3d direction) return Math.Sqrt(sqrMagnitude); } + public Vector3d SelectAxis(Vector3d other) + { + return new Vector3d + { + x = other.x > 0 ? positive.x : negative.x, + y = other.y > 0 ? positive.y : negative.y, + z = other.z > 0 ? positive.z : negative.z, + }; + } + + public Vector3d ToVector3d() + { + return Vector3d.Max(positive,negative); + } + public double MaxMagnitude() { return Math.Max(positive.MaxMagnitude(),negative.MaxMagnitude()); @@ -124,6 +138,17 @@ public void Load(ConfigNode node) } } + public static Vector6 operator +(Vector6 a,Vector6 b) + { + Vector6 sum = new Vector6 + { + positive = a.positive + b.positive, + negative = a.negative + b.negative + }; + + return sum; + } + public void Save(ConfigNode node) { node.SetValue("positive", KSPUtil.WriteVector(positive)); diff --git a/MechJeb2/VesselState.cs b/MechJeb2/VesselState.cs index ec370b217..dbc49eb50 100644 --- a/MechJeb2/VesselState.cs +++ b/MechJeb2/VesselState.cs @@ -1,7 +1,6 @@ using System; using System.Collections; using System.Collections.Generic; -using System.Linq; using Smooth.Pools; using UnityEngine; using System.Reflection; @@ -272,7 +271,7 @@ public double currentHeading // Total torque - public Vector3d torqueAvailable; + public Vector6 torqueAvailable; public Vector3d torqueReactionSpeed; @@ -922,7 +921,7 @@ public void RCSTorque() private const double _dVelocitySqrMinThreshold = 1; private const double _dAltitudeThreshold = 300; private const float _fAoAThreshold = 2; - private void CalculateVesselAeroForcesWithCache(Vessel v, out Vector3 farForce, out Vector3 farTorque, Vector3d surfaceVelocity, double altitudeASL) + private void CalculateVesselAeroForcesWithCache(Vessel v, out Vector3 farForce, out Vector3 farTorque, Vector3d surfaceVelocity, double altitudeASL) { float AoA = Vector3.Angle(v.rootPart.transform.TransformDirection(Vector3.up), surfaceVelocity); if ((lastSurfaceVelocity - surfaceVelocity).sqrMagnitude > _dVelocitySqrThreshold @@ -948,7 +947,7 @@ void AnalyzeParts(Vessel vessel, EngineInfo einfo, IntakeInfo iinfo) parachutes.Clear(); parachuteDeployed = false; - torqueAvailable = Vector3d.zero; + torqueAvailable = new Vector6(); Vector6 torqueReactionSpeed6 = new Vector6(); @@ -1035,8 +1034,8 @@ void AnalyzeParts(Vessel vessel, EngineInfo einfo, IntakeInfo iinfo) rw.GetPotentialTorque(out pos, out neg); // GetPotentialTorque reports the same value for pos & neg on ModuleReactionWheel - torqueReactionWheel.Add(pos); - torqueReactionWheel.Add(-neg); + torqueReactionWheel.Add(pos.Abs()); + torqueReactionWheel.Add(-neg.Abs()); } else if (pm is ModuleEngines) { @@ -1072,8 +1071,8 @@ void AnalyzeParts(Vessel vessel, EngineInfo einfo, IntakeInfo iinfo) cs.GetPotentialTorque(out ctrlTorquePos, out ctrlTorqueNeg); - torqueControlSurface.Add(ctrlTorquePos); - torqueControlSurface.Add(ctrlTorqueNeg); + torqueControlSurface.Add(ctrlTorquePos.Abs()); + torqueControlSurface.Add(-ctrlTorqueNeg.Abs()); torqueReactionSpeed6.Add(Mathf.Abs(cs.ctrlSurfaceRange) / cs.actuatorSpeed * Vector3d.Max(ctrlTorquePos.Abs(), ctrlTorqueNeg.Abs())); } @@ -1099,8 +1098,8 @@ void AnalyzeParts(Vessel vessel, EngineInfo einfo, IntakeInfo iinfo) // GetPotentialTorque reports the same value for pos & neg on ModuleGimbal - torqueGimbal.Add(pos); - torqueGimbal.Add(-neg); + torqueGimbal.Add(pos.Abs()); + torqueGimbal.Add(-neg.Abs()); if (g.useGimbalResponseSpeed) torqueReactionSpeed6.Add((Mathf.Abs(g.gimbalRange) / g.gimbalResponseSpeed) * Vector3d.Max(pos.Abs(), neg.Abs())); @@ -1117,8 +1116,8 @@ void AnalyzeParts(Vessel vessel, EngineInfo einfo, IntakeInfo iinfo) Vector3 neg; tp.GetPotentialTorque(out pos, out neg); - torqueOthers.Add(pos); - torqueOthers.Add(neg); + torqueOthers.Add(pos.Abs()); + torqueOthers.Add(-neg.Abs()); } for (int index = 0; index < vesselStatePartModuleExtensions.Count; index++) @@ -1154,25 +1153,25 @@ void AnalyzeParts(Vessel vessel, EngineInfo einfo, IntakeInfo iinfo) } } - torqueAvailable += Vector3d.Max(torqueReactionWheel.positive, torqueReactionWheel.negative); + torqueAvailable += torqueReactionWheel; //torqueAvailable += Vector3d.Max(torqueRcs.positive, torqueRcs.negative); - torqueAvailable += Vector3d.Max(rcsTorqueAvailable.positive, rcsTorqueAvailable.negative); + torqueAvailable += rcsTorqueAvailable; - torqueAvailable += Vector3d.Max(torqueControlSurface.positive, torqueControlSurface.negative); + torqueAvailable += torqueControlSurface; - torqueAvailable += Vector3d.Max(torqueGimbal.positive, torqueGimbal.negative); + torqueAvailable += torqueGimbal; - torqueAvailable += Vector3d.Max(torqueOthers.positive, torqueOthers.negative); // Mostly FAR + torqueAvailable += torqueOthers; torqueDiffThrottle = Vector3d.Max(einfo.torqueDiffThrottle.positive, einfo.torqueDiffThrottle.negative); torqueDiffThrottle.y = 0; - if (torqueAvailable.sqrMagnitude > 0) + if (torqueAvailable.ToVector3d().sqrMagnitude > 0) { torqueReactionSpeed = Vector3d.Max(torqueReactionSpeed6.positive, torqueReactionSpeed6.negative); - torqueReactionSpeed.Scale(torqueAvailable.InvertNoNaN()); + torqueReactionSpeed.Scale(torqueAvailable.ToVector3d().InvertNoNaN()); } else {