Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix much Torquestuff #1542

Open
wants to merge 2 commits into
base: dev
Choose a base branch
from
Open

Fix much Torquestuff #1542

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
59c6d20
Fix much Torquestuff
lamont-granquist Apr 25, 2022
4aa1e88
fix some details
lamont-granquist Apr 25, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions MechJeb2/AttitudeControllers/BetterController.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion MechJeb2/AttitudeControllers/HybridController.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
14 changes: 7 additions & 7 deletions MechJeb2/AttitudeControllers/KosAttitudeController.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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)
{
Expand All @@ -52,15 +52,15 @@ 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;
private Vector3d vesselTop;
private Vector3d vesselStarboard;
private Vector3d targetForward;
private Vector3d targetTop;

/* private Vector3d targetStarboard; */

private void UpdateStateVectors() {
Expand Down Expand Up @@ -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];
}
Expand Down Expand Up @@ -135,7 +135,7 @@ public override void Reset()
yawRatePI.ResetI();
rollRatePI.ResetI();
}


private void UpdateControl() {
/* TODO: static engine torque and/or differential throttle */
Expand Down
4 changes: 2 additions & 2 deletions MechJeb2/AttitudeControllers/MJAttitudeController.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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:
Expand Down
4 changes: 2 additions & 2 deletions MechJeb2/MechJebModuleAttitudeAdjustment.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand All @@ -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
Expand Down
6 changes: 3 additions & 3 deletions MechJeb2/MechJebModuleAttitudeController.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ private set

public double attitudeError;

public Vector3d torque;
public Vector6 torque;
public Vector3d inertia;

public MechJebModuleAttitudeController(MechJebCore core)
Expand Down Expand Up @@ -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();
Expand Down
2 changes: 1 addition & 1 deletion MechJeb2/MechJebModuleRoverController.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand Down
8 changes: 4 additions & 4 deletions MechJeb2/MechJebModuleThrustController.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)))
{
Expand Down
8 changes: 7 additions & 1 deletion MechJeb2/MuUtils.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) + "]";
Expand Down Expand Up @@ -171,7 +177,7 @@ public static bool PhysicsRunning()
{
return (TimeWarp.WarpMode == TimeWarp.Modes.LOW) || (TimeWarp.CurrentRateIndex == 0);
}

public static string SystemClipboard
{
get => GUIUtility.systemCopyBuffer;
Expand Down
31 changes: 28 additions & 3 deletions MechJeb2/Vector6.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 };

Expand Down Expand Up @@ -66,7 +66,6 @@ public double this[Direction index]
}
}

public Vector6() { }
public Vector6(Vector3d positive, Vector3d negative)
{
this.positive = positive;
Expand Down Expand Up @@ -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());
Expand All @@ -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));
Expand Down
37 changes: 18 additions & 19 deletions MechJeb2/VesselState.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using Smooth.Pools;
using UnityEngine;
using System.Reflection;
Expand Down Expand Up @@ -272,7 +271,7 @@ public double currentHeading


// Total torque
public Vector3d torqueAvailable;
public Vector6 torqueAvailable;

public Vector3d torqueReactionSpeed;

Expand Down Expand Up @@ -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
Expand All @@ -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();

Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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()));
}
Expand All @@ -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()));
Expand All @@ -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++)
Expand Down Expand Up @@ -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
{
Expand Down