public void tuneTf(Vector3d torque)
{
Vector3d ratio = new Vector3d(
torque.x != 0 ? vesselState.MoI.x / torque.x : 0,
torque.z != 0 ? vesselState.MoI.z / torque.z : 0, //y <=> z
torque.y != 0 ? vesselState.MoI.y / torque.y : 0 //z <=> y
);
TfV = 0.05 * ratio;
Vector3d delayFactor = Vector3d.one + 2 * vesselState.torqueReactionSpeed;
TfV.Scale(delayFactor);
TfV = TfV.Clamp(2.0 * TimeWarp.fixedDeltaTime, 1.0);
TfV = TfV.Clamp(TfMin, TfMax);
//Tf = Mathf.Clamp((float)ratio.magnitude / 20f, 2 * TimeWarp.fixedDeltaTime, 1f);
//Tf = Mathf.Clamp((float)Tf, (float)TfMin, (float)TfMax);
}
public void Drive(FlightCtrlState s)
{
if (useSAS)
{
_requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Quaternion.Euler(90, 0, 0);
if (!vessel.ActionGroups[KSPActionGroup.SAS])
{
vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true);
vessel.Autopilot.SAS.LockHeading(_requestedAttitude);
lastSAS = _requestedAttitude;
}
else if (Quaternion.Angle(lastSAS, _requestedAttitude) > 10)
{
vessel.Autopilot.SAS.LockHeading(_requestedAttitude);
lastSAS = _requestedAttitude;
}
else
{
vessel.Autopilot.SAS.LockHeading(_requestedAttitude, true);
}
}
else
{
// Direction we want to be facing
_requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget;
Transform vesselTransform = vessel.ReferenceTransform;
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselTransform.rotation) * _requestedAttitude);
Vector3d deltaEuler = delta.DeltaEuler();
// ( MoI / available torque ) factor:
Vector3d NormFactor = Vector3d.Scale(vesselState.MoI, torque.Invert()).Reorder(132);
// Find out the real shorter way to turn were we wan to.
// Thanks to HoneyFox
Vector3d tgtLocalUp = vesselTransform.transform.rotation.Inverse() * _requestedAttitude * Vector3d.forward;
Vector3d curLocalUp = Vector3d.up;
double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
rotDirection = rotDirection.normalized * turnAngle / 180.0;
// And the lowest roll
// Thanks to Crzyrndm
Vector3 normVec = Vector3.Cross(_requestedAttitude * Vector3.forward, vesselTransform.up);
Quaternion targetDeRotated = Quaternion.AngleAxis((float)turnAngle, normVec) * _requestedAttitude;
float rollError = Vector3.Angle(vesselTransform.right, targetDeRotated * Vector3.right) * Math.Sign(Vector3.Dot(targetDeRotated * Vector3.right, vesselTransform.forward));
error = new Vector3d(
-rotDirection.y * Math.PI,
rotDirection.x * Math.PI,
rollError * Mathf.Deg2Rad
);
error.Scale(_axisControl);
Vector3d err = error + inertia.Reorder(132) / 2d;
err = new Vector3d(
Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));
err.Scale(NormFactor);
// angular velocity:
Vector3d omega;
omega.x = vessel.angularVelocity.x;
omega.y = vessel.angularVelocity.z; // y <=> z
omega.z = vessel.angularVelocity.y; // z <=> y
omega.Scale(NormFactor);
if (Tf_autoTune)
tuneTf(torque);
setPIDParameters();
// angular velocity limit:
var Wlimit = new Vector3d(Math.Sqrt(NormFactor.x * Math.PI * kWlimit),
Math.Sqrt(NormFactor.y * Math.PI * kWlimit),
Math.Sqrt(NormFactor.z * Math.PI * kWlimit));
pidAction = pid.Compute(err, omega, Wlimit);
// deadband
pidAction.x = Math.Abs(pidAction.x) >= deadband ? pidAction.x : 0.0f;
pidAction.y = Math.Abs(pidAction.y) >= deadband ? pidAction.y : 0.0f;
pidAction.z = Math.Abs(pidAction.z) >= deadband ? pidAction.z : 0.0f;
// low pass filter, wf = 1/Tf:
Vector3d act = lastAct;
act.x += (pidAction.x - lastAct.x) * (1.0 / ((TfV.x / TimeWarp.fixedDeltaTime) + 1.0));
act.y += (pidAction.y - lastAct.y) * (1.0 / ((TfV.y / TimeWarp.fixedDeltaTime) + 1.0));
act.z += (pidAction.z - lastAct.z) * (1.0 / ((TfV.z / TimeWarp.fixedDeltaTime) + 1.0));
lastAct = act;
SetFlightCtrlState(act, deltaEuler, s, 1);
act = new Vector3d(s.pitch, s.yaw, s.roll);
// Feed the control torque to the differential throttle
}
}
private void drive(FlightCtrlState s)
{
stress = 0;
if (part.State == PartStates.DEAD)
{
return;
}
if (controlModule != null)
{
if (controlModule.enabled)
{
controlModule.drive(s);
}
else
{
_controlModule = null;
}
}
if (TimeWarp.CurrentRate > TimeWarp.MaxPhysicsRate)
{
return;
}
if ((tmode != TMode.OFF) && (vesselState.thrustAvailable > 0))
{
double spd = 0;
if (trans_land)
{
if (part.vessel.Landed || part.vessel.Splashed)
{
tmode = TMode.OFF;
trans_land = false;
}
else
{
tmode = TMode.KEEP_VERTICAL;
trans_kill_h = true;
double minalt = Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue);
if (vesselState.maxThrustAccel < vesselState.gravityForce.magnitude)
{
trans_spd_act = 0;
}
else
{
trans_spd_act = (float)-Math.Sqrt((vesselState.maxThrustAccel - vesselState.gravityForce.magnitude) * 2 * minalt) * 0.50F;
}
if (!trans_land_gears && (minalt < 1000))
{
foreach (Part p in part.vessel.parts)
{
if (p is LandingLeg)
{
LandingLeg l = (LandingLeg)p;
if (l.legState == LandingLeg.LegStates.RETRACTED)
{
l.DeployOnActivate = true;
l.force_activate();
}
}
}
trans_land_gears = true;
}
if (minalt < 200)
{
minalt = Math.Min(vesselState.altitudeBottom, minalt);
trans_spd_act = -Mathf.Lerp(0, (float)Math.Sqrt((vesselState.maxThrustAccel - vesselState.gravityForce.magnitude) * 2 * 200) * 0.50F, (float)minalt / 200);
trans_spd_act = (float)Math.Min(-trans_land_touchdown_speed, trans_spd_act);
// if (minalt < 3.0 * trans_land_touchdown_speed) trans_spd_act = -(float)trans_land_touchdown_speed;
/*
if ((minalt >= 90) && (vesselState.speedHorizontal > 1)) {
trans_spd_act = -Mathf.Lerp(0, (float)Math.Sqrt((vesselState.maxThrustAccel - vesselState.gravityForce.magnitude) * 2 * 200) * 0.90F, (float)(minalt - 100) / 300);
} else {
trans_spd_act = -Mathf.Lerp(0, (float)Math.Sqrt((vesselState.maxThrustAccel - vesselState.gravityForce.magnitude) * 2 * 200) * 0.90F, (float)minalt / 300);
}
*/
}
}
}
switch (tmode)
{
case TMode.KEEP_ORBITAL:
spd = vesselState.speedOrbital;
break;
case TMode.KEEP_SURFACE:
spd = vesselState.speedSurface;
break;
case TMode.KEEP_VERTICAL:
spd = vesselState.speedVertical;
Vector3d rot = Vector3d.up;
if (trans_kill_h)
{
Vector3 hsdir = Vector3.Exclude(vesselState.up, part.vessel.orbit.GetVel() - part.vessel.mainBody.getRFrmVel(vesselState.CoM));
Vector3 dir = -hsdir + vesselState.up * Math.Max(Math.Abs(spd), 20 * part.vessel.mainBody.GeeASL);
if ((Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue) > 5000) && (hsdir.magnitude > Math.Max(Math.Abs(spd), 100 * part.vessel.mainBody.GeeASL) * 2))
{
tmode = TMode.DIRECT;
trans_spd_act = 100;
rot = -hsdir;
}
else
{
if (spd > 0)
{
rot = vesselState.up;
}
else
{
rot = dir.normalized;
}
}
attitudeTo(rot, AttitudeReference.INERTIAL, null);
}
break;
}
double t_err = (trans_spd_act - spd) / vesselState.maxThrustAccel;
t_integral += t_err * TimeWarp.fixedDeltaTime;
double t_deriv = (t_err - t_prev_err) / TimeWarp.fixedDeltaTime;
double t_act = (t_Kp * t_err) + (t_Ki * t_integral) + (t_Kd * t_deriv);
t_prev_err = t_err;
double int_error = Math.Abs(Vector3d.Angle(attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Vector3d.forward, vesselState.forward));
//print("int_error = " + int_error);
if ((tmode != TMode.KEEP_VERTICAL) || (int_error < 2) || ((Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue) < 1000) && (int_error < 90)))
{
if (tmode == TMode.DIRECT)
{
trans_prev_thrust = s.mainThrottle = trans_spd_act / 100.0F;
}
else
{
trans_prev_thrust = s.mainThrottle = Mathf.Clamp(trans_prev_thrust + (float)t_act, 0, 1.0F);
}
}
else
{
if ((int_error >= 2) && (vesselState.torqueThrustPYAvailable > vesselState.torquePYAvailable * 10))
{
trans_prev_thrust = s.mainThrottle = 0.1F;
}
else
{
trans_prev_thrust = s.mainThrottle = 0;
}
}
}
if (attitudeActive)
{
int userCommanding = (Mathfx.Approx(s.pitch, 0, 0.1F) ? 0 : 1) + (Mathfx.Approx(s.yaw, 0, 0.1F) ? 0 : 2) + (Mathfx.Approx(s.roll, 0, 0.1F) ? 0 : 4);
s.killRot = false;
Quaternion target = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget;
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(part.vessel.transform.rotation) * target);
/*
if (Mathf.Abs(Quaternion.Angle(delta, Quaternion.identity)) > 30) {
delta = Quaternion.Slerp(Quaternion.identity, delta, 0.1F);
}
*/
Vector3d deltaEuler = delta.eulerAngles;
Vector3d err = new Vector3d((deltaEuler.x > 180) ? (deltaEuler.x - 360.0F) : deltaEuler.x, -((deltaEuler.y > 180) ? (deltaEuler.y - 360.0F) : deltaEuler.y), (deltaEuler.z > 180) ? (deltaEuler.z - 360.0F) : deltaEuler.z) * Math.PI / 180.0F;
Vector3d torque = new Vector3d(vesselState.torquePYAvailable + vesselState.torqueThrustPYAvailable * s.mainThrottle, vesselState.torqueRAvailable, vesselState.torquePYAvailable + vesselState.torqueThrustPYAvailable * s.mainThrottle);
Vector3d inertia = Vector3d.Scale(MuUtils.Sign(vesselState.angularMomentum) * 1.1, Vector3d.Scale(Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum), MuUtils.Invert(Vector3d.Scale(torque, vesselState.MoI))));
err += MuUtils.Reorder(inertia, 132);
err.Scale(Vector3d.Scale(vesselState.MoI, MuUtils.Invert(torque)));
integral += err * TimeWarp.fixedDeltaTime;
Vector3d deriv = (err - prev_err) / TimeWarp.fixedDeltaTime;
act = Kp * err + Ki * integral + Kd * deriv;
prev_err = err;
if (userCommanding != 0)
{
if (attitudeKILLROT)
{
attitudeTo(Quaternion.LookRotation(part.vessel.transform.up, -part.vessel.transform.forward), AttitudeReference.INERTIAL, null);
}
}
if ((userCommanding & 4) > 0)
{
prev_err = new Vector3d(prev_err.x, prev_err.y, 0);
integral = new Vector3d(integral.x, integral.y, 0);
if (!attitudeRollMatters)
{
attitudeTo(Quaternion.LookRotation(attitudeTarget * Vector3d.forward, attitudeWorldToReference(-part.vessel.transform.forward, attitudeReference)), attitudeReference, null);
_attitudeRollMatters = false;
}
}
else
{
if (!double.IsNaN(act.z)) s.roll = Mathf.Clamp(s.roll + act.z, -1.0F, 1.0F);
}
if ((userCommanding & 3) > 0)
{
prev_err = new Vector3d(0, 0, prev_err.z);
integral = new Vector3d(0, 0, integral.z);
}
else
{
if (!double.IsNaN(act.x)) s.pitch = Mathf.Clamp(s.pitch + act.x, -1.0F, 1.0F);
if (!double.IsNaN(act.y)) s.yaw = Mathf.Clamp(s.yaw + act.y, -1.0F, 1.0F);
}
stress = Mathf.Min(Mathf.Abs(act.x), 1.0F) + Mathf.Min(Mathf.Abs(act.y), 1.0F) + Mathf.Min(Mathf.Abs(act.z), 1.0F);
}
if (double.IsNaN(s.mainThrottle))
{
print("MechJeb: caught throttle = NaN");
s.mainThrottle = 0;
}
}
/// <summary>
/// Automatically guides the ship to face a desired orientation
/// </summary>
/// <param name="target">The desired orientation</param>
/// <param name="c">The FlightCtrlState for the current vessel.</param>
/// <param name="fc">The flight computer carrying out the slew</param>
/// <param name="ignoreRoll">[optional] to ignore the roll</param>
public static void SteerShipToward(Quaternion target, FlightCtrlState c, FlightComputer fc, bool ignoreRoll)
{
// Add support for roll-less targets later -- Starstrider42
bool fixedRoll = !ignoreRoll;
Vessel vessel = fc.Vessel;
Vector3d momentOfInertia = GetTrueMoI(vessel);
Transform vesselReference = vessel.GetTransform();
//---------------------------------------
// Copied almost verbatim from MechJeb master on June 27, 2014 -- Starstrider42
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target);
Vector3d torque = GetTorque(vessel, c.mainThrottle);
Vector3d spinMargin = GetStoppingAngle(vessel, torque);
// Allow for zero torque around some but not all axes
Vector3d normFactor;
normFactor.x = (torque.x != 0 ? momentOfInertia.x / torque.x : 0.0);
normFactor.y = (torque.y != 0 ? momentOfInertia.y / torque.y : 0.0);
normFactor.z = (torque.z != 0 ? momentOfInertia.z / torque.z : 0.0);
normFactor = SwapYZ(normFactor);
// Find out the real shorter way to turn were we want to.
// Thanks to HoneyFox
Vector3d tgtLocalUp = vesselReference.transform.rotation.Inverse() * target * Vector3d.forward;
Vector3d curLocalUp = Vector3d.up;
double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
var rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
rotDirection = rotDirection.normalized * turnAngle / 180.0f;
var err = new Vector3d(
-rotDirection.y * Math.PI,
rotDirection.x * Math.PI,
fixedRoll ?
((delta.eulerAngles.z > 180) ?
(delta.eulerAngles.z - 360.0F) :
delta.eulerAngles.z) * Math.PI / 180.0F
: 0F
);
err += SwapYZ(spinMargin);
err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));
err.Scale(normFactor);
// angular velocity:
Vector3d omega = SwapYZ(vessel.angularVelocity);
omega.Scale(normFactor);
Vector3d pidAction = fc.pid.Compute(err, omega);
// low pass filter, wf = 1/Tf:
Vector3d act = fc.lastAct + (pidAction - fc.lastAct) * (1 / ((fc.Tf / TimeWarp.fixedDeltaTime) + 1));
fc.lastAct = act;
// end MechJeb import
//---------------------------------------
float precision = Mathf.Clamp((float)(torque.x * 20f / momentOfInertia.magnitude), 0.5f, 10f);
float driveLimit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision));
act.x = Mathf.Clamp((float)act.x, -driveLimit, driveLimit);
act.y = Mathf.Clamp((float)act.y, -driveLimit, driveLimit);
act.z = Mathf.Clamp((float)act.z, -driveLimit, driveLimit);
c.roll = Mathf.Clamp((float)(c.roll + act.z), -driveLimit, driveLimit);
c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -driveLimit, driveLimit);
c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -driveLimit, driveLimit);
}
public override void Drive(FlightCtrlState s)
{
if (useSAS)
{
_requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Quaternion.Euler(90, 0, 0);
if (!part.vessel.ActionGroups[KSPActionGroup.SAS])
{
part.vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true);
part.vessel.Autopilot.SAS.LockHeading(_requestedAttitude);
lastSAS = _requestedAttitude;
}
else if (Quaternion.Angle(lastSAS, _requestedAttitude) > 10)
{
part.vessel.Autopilot.SAS.LockHeading(_requestedAttitude);
lastSAS = _requestedAttitude;
}
else
{
part.vessel.Autopilot.SAS.LockHeading(_requestedAttitude, true);
}
core.thrust.differentialThrottleDemandedTorque = Vector3d.zero;
}
else
{
// Direction we want to be facing
_requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget;
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * _requestedAttitude);
Vector3d deltaEuler = new Vector3d(
(delta.eulerAngles.x > 180) ? (delta.eulerAngles.x - 360.0F) : delta.eulerAngles.x,
-((delta.eulerAngles.y > 180) ? (delta.eulerAngles.y - 360.0F) : delta.eulerAngles.y),
(delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z
);
Vector3d torque = vesselState.torqueAvailable + vesselState.torqueFromEngine * vessel.ctrlState.mainThrottle;
if (core.thrust.differentialThrottleSuccess)
torque += vesselState.torqueFromDiffThrottle * vessel.ctrlState.mainThrottle / 2;
Vector3d inertia = Vector3d.Scale(
vesselState.angularMomentum.Sign(),
Vector3d.Scale(
Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum),
Vector3d.Scale(torque, vesselState.MoI).Invert()
)
);
// ( MoI / avaiable torque ) factor:
Vector3d NormFactor = Vector3d.Scale(vesselState.MoI, torque.Invert()).Reorder(132);
// Find out the real shorter way to turn were we wan to.
// Thanks to HoneyFox
Vector3d tgtLocalUp = vessel.ReferenceTransform.transform.rotation.Inverse() * _requestedAttitude * Vector3d.forward;
Vector3d curLocalUp = Vector3d.up;
double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
rotDirection = rotDirection.normalized * turnAngle / 180.0f;
Vector3d err = new Vector3d(
-rotDirection.y * Math.PI,
rotDirection.x * Math.PI,
attitudeRollMatters ? ((delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z) * Math.PI / 180.0F : 0F
);
err += inertia.Reorder(132) / 2;
err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));
err.Scale(NormFactor);
// angular velocity:
Vector3d omega;
omega.x = vessel.angularVelocity.x;
omega.y = vessel.angularVelocity.z; // y <=> z
omega.z = vessel.angularVelocity.y; // z <=> y
omega.Scale(NormFactor);
pidAction = pid.Compute(err, omega);
// low pass filter, wf = 1/Tf:
Vector3d act = lastAct + (pidAction - lastAct) * (1 / ((Tf / TimeWarp.fixedDeltaTime) + 1));
lastAct = act;
SetFlightCtrlState(act, deltaEuler, s, 1);
act = new Vector3d(s.pitch, s.yaw, s.roll);
// Feed the control torque to the differential throttle
if (core.thrust.differentialThrottleSuccess)
core.thrust.differentialThrottleDemandedTorque = -Vector3d.Scale(act.xzy, vesselState.torqueFromDiffThrottle * vessel.ctrlState.mainThrottle);
}
}
public void UpdateStateVectors()
{
Direction targetdir = GetDirectionFromValue();
if (targetdir == null)
{
shared.Logger.LogWarning(string.Format("SteeringManager target direction is null, Value = {0}", Value));
return;
}
targetRot = targetdir.Rotation;
centerOfMass = shared.Vessel.findWorldCenterOfMass();
vesselTransform = shared.Vessel.ReferenceTransform;
// Found that the default rotation has top pointing forward, forward pointing down, and right pointing starboard.
// This fixes that rotation.
vesselRotation = vesselTransform.rotation * Quaternion.Euler(-90, 0, 0);
vesselForward = vesselRotation * Vector3d.forward;
vesselTop = vesselRotation * Vector3d.up;
vesselStarboard = vesselRotation * Vector3d.right;
vesselUp = (centerOfMass - shared.Vessel.mainBody.position).normalized;
targetForward = targetRot * Vector3d.forward;
targetTop = targetRot * Vector3d.up;
targetStarboard = targetRot * Vector3d.right;
Vector3d oldOmega = omega;
// omega is angular rotation. need to correct the signs to agree with the facing axis
omega = Quaternion.Inverse(vesselRotation) * shared.Vessel.rigidbody.angularVelocity;
omega.x *= -1; //positive values pull the nose to the starboard.
//omega.y *= -1; // positive values pull the nose up.
omega.z *= -1; // positive values pull the starboard side up.
// TODO: Currently adjustments to MOI are only enabled in debug compiles. Using this feature seems to be buggy, but it has potential
// to be more resiliant against random spikes in angular velocity.
if (sessionTime > lastSessionTime)
{
double dt = sessionTime - lastSessionTime;
angularAcceleration = (omega - oldOmega) / dt;
angularAcceleration = new Vector3d(angularAcceleration.x, angularAcceleration.z, angularAcceleration.y);
if (enableMoiAdjust)
{
measuredMomentOfInertia = new Vector3d(
controlTorque.x * accPitch / angularAcceleration.x,
controlTorque.y * accRoll / angularAcceleration.y,
controlTorque.z * accYaw / angularAcceleration.z);
if (Math.Abs(accPitch) > EPSILON)
{
adjustMomentOfInertia.x = pitchMOICalc.Update(Math.Abs(measuredMomentOfInertia.x) / momentOfInertia.x);
}
if (Math.Abs(accYaw) > EPSILON)
{
adjustMomentOfInertia.z = yawMOICalc.Update(Math.Abs(measuredMomentOfInertia.z) / momentOfInertia.z);
}
if (Math.Abs(accRoll) > EPSILON)
{
adjustMomentOfInertia.y = rollMOICalc.Update(Math.Abs(measuredMomentOfInertia.y) / momentOfInertia.y);
}
}
}
momentOfInertia = shared.Vessel.findLocalMOI(centerOfMass);
momentOfInertia.Scale(adjustMomentOfInertia);
adjustTorque = Vector3d.zero;
measuredTorque = Vector3d.Scale(momentOfInertia, angularAcceleration);
if (sessionTime > lastSessionTime && EnableTorqueAdjust)
{
if (Math.Abs(accPitch) > EPSILON)
{
adjustTorque.x = Math.Min(Math.Abs(pitchTorqueCalc.Update(measuredTorque.x / accPitch)) - rawTorque.x, 0);
//adjustTorque.x = Math.Abs(pitchTorqueCalc.Update(measuredTorque.x / accPitch) / rawTorque.x);
}
else adjustTorque.x = Math.Abs(pitchTorqueCalc.Update(pitchTorqueCalc.Mean));
if (Math.Abs(accYaw) > EPSILON)
{
adjustTorque.z = Math.Min(Math.Abs(yawTorqueCalc.Update(measuredTorque.z / accYaw)) - rawTorque.z, 0);
//adjustTorque.z = Math.Abs(yawTorqueCalc.Update(measuredTorque.z / accYaw) / rawTorque.z);
}
else adjustTorque.z = Math.Abs(yawTorqueCalc.Update(yawTorqueCalc.Mean));
if (Math.Abs(accRoll) > EPSILON)
{
adjustTorque.y = Math.Min(Math.Abs(rollTorqueCalc.Update(measuredTorque.y / accRoll)) - rawTorque.y, 0);
//adjustTorque.y = Math.Abs(rollTorqueCalc.Update(measuredTorque.y / accRoll) / rawTorque.y);
}
else adjustTorque.y = Math.Abs(rollTorqueCalc.Update(rollTorqueCalc.Mean));
}
}
public override void Drive(FlightCtrlState s)
{
if (useSAS)
{
Quaternion target = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Quaternion.Euler(90, 0, 0);
if (!part.vessel.ActionGroups[KSPActionGroup.SAS])
{
part.vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true);
part.vessel.VesselSAS.LockHeading(target);
lastSAS = target;
}
else if (Quaternion.Angle(lastSAS, target) > 10)
{
part.vessel.VesselSAS.LockHeading(target);
lastSAS = target;
}
else
{
part.vessel.VesselSAS.LockHeading(target, true);
}
}
else
{
// Direction we want to be facing
Quaternion target = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget;
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * target);
Vector3d deltaEuler = new Vector3d(
(delta.eulerAngles.x > 180) ? (delta.eulerAngles.x - 360.0F) : delta.eulerAngles.x,
-((delta.eulerAngles.y > 180) ? (delta.eulerAngles.y - 360.0F) : delta.eulerAngles.y),
(delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z
);
Vector3d torque = new Vector3d(
vesselState.torqueAvailable.x + vesselState.torqueThrustPYAvailable * s.mainThrottle,
vesselState.torqueAvailable.y,
vesselState.torqueAvailable.z + vesselState.torqueThrustPYAvailable * s.mainThrottle
);
Vector3d inertia = Vector3d.Scale(
vesselState.angularMomentum.Sign(),
Vector3d.Scale(
Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum),
Vector3d.Scale(torque, vesselState.MoI).Invert()
)
);
// ( MoI / avaiable torque ) factor:
Vector3d NormFactor = Vector3d.Scale(vesselState.MoI, torque.Invert()).Reorder(132);
// angular error:
Vector3d err = deltaEuler * Math.PI / 180.0F;
err += inertia.Reorder(132) / 2;
err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));
err.Scale(NormFactor);
// angular velocity:
Vector3d omega;
omega.x = vessel.angularVelocity.x;
omega.y = vessel.angularVelocity.z; // y <=> z
omega.z = vessel.angularVelocity.y; // z <=> y
omega.Scale(NormFactor);
pidAction = pid.Compute(err, omega);
// low pass filter, wf = 1/Tf:
Vector3d act = lastAct + (pidAction - lastAct) * (1 / ((Tf / TimeWarp.fixedDeltaTime) + 1));
lastAct = act;
SetFlightCtrlState(act, deltaEuler, s, 1);
act = new Vector3d(s.pitch, s.yaw, s.roll);
}
}
public Vector3d Compute(Vector3d error, Vector3d omega)
{
derivativeAct = omega;
derivativeAct.Scale(Kd);
// integral actíon + Anti Windup
intAccum.x = (Math.Abs(derivativeAct.x) < 0.6 * max) ? intAccum.x + (error.x * Ki.x * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.x;
intAccum.y = (Math.Abs(derivativeAct.y) < 0.6 * max) ? intAccum.y + (error.y * Ki.y * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.y;
intAccum.z = (Math.Abs(derivativeAct.z) < 0.6 * max) ? intAccum.z + (error.z * Ki.z * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.z;
propAct = error;
propAct.Scale(Kp);
Vector3d action = propAct + derivativeAct + intAccum;
// action clamp
action = new Vector3d(Math.Max(min, Math.Min(max, action.x)),
Math.Max(min, Math.Min(max, action.y)),
Math.Max(min, Math.Min(max, action.z)));
return action;
}
/// <summary>
/// Automatically guides the ship to face a desired orientation
/// </summary>
/// <param name="target">The desired orientation</param>
/// <param name="c">The FlightCtrlState for the current vessel.</param>
/// <param name="fc">The flight computer carrying out the slew</param>
/// <param name="ignoreRoll">[optional] to ignore the roll</param>
public static void SteerShipToward(Quaternion target, FlightCtrlState c, FlightComputer fc, bool ignoreRoll)
{
// Add support for roll-less targets later -- Starstrider42
bool fixedRoll = !ignoreRoll;
Vessel vessel = fc.Vessel;
Vector3d momentOfInertia = vessel.MOI;
Transform vesselReference = vessel.GetTransform();
Vector3d torque = GetVesselTorque(vessel);
// -----------------------------------------------
// Copied from MechJeb master on 18.04.2016 with some modifications to adapt to RemoteTech
Vector3d _axisControl = new Vector3d();
_axisControl.x = true ? 1 : 0;
_axisControl.y = true ? 1 : 0;
_axisControl.z = fixedRoll ? 1 : 0;
Vector3d inertia = Vector3d.Scale(
new Vector3d(vessel.angularMomentum.x, vessel.angularMomentum.y, vessel.angularMomentum.z).Sign(),
Vector3d.Scale(
Vector3d.Scale(vessel.angularMomentum, vessel.angularMomentum),
Vector3d.Scale(torque, momentOfInertia).Invert()
)
);
Vector3d TfV = new Vector3d(0.3, 0.3, 0.3);
double kpFactor = 3;
double kiFactor = 6;
double kdFactor = 0.5;
double kWlimit = 0.15;
double deadband = 0.0001;
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target);
Vector3d deltaEuler = delta.DeltaEuler();
// ( MoI / available torque ) factor:
Vector3d NormFactor = Vector3d.Scale(momentOfInertia, torque.Invert()).Reorder(132);
// Find out the real shorter way to turn were we want to.
// Thanks to HoneyFox
Vector3d tgtLocalUp = vesselReference.rotation.Inverse() * target * Vector3d.forward;
Vector3d curLocalUp = Vector3d.up;
double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
rotDirection = rotDirection.normalized * turnAngle / 180.0;
// And the lowest roll
// Thanks to Crzyrndm
Vector3 normVec = Vector3.Cross(target * Vector3.forward, vesselReference.up);
Quaternion targetDeRotated = Quaternion.AngleAxis((float)turnAngle, normVec) * target;
float rollError = Vector3.Angle(vesselReference.right, targetDeRotated * Vector3.right) * Math.Sign(Vector3.Dot(targetDeRotated * Vector3.right, vesselReference.forward));
var error = new Vector3d(
-rotDirection.y * Math.PI,
rotDirection.x * Math.PI,
rollError * Mathf.Deg2Rad
);
error.Scale(_axisControl);
Vector3d err = error + inertia.Reorder(132) / 2d;
err = new Vector3d(
Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));
err.Scale(NormFactor);
// angular velocity:
Vector3d omega;
omega.x = vessel.angularVelocity.x;
omega.y = vessel.angularVelocity.z; // y <=> z
omega.z = vessel.angularVelocity.y; // z <=> y
omega.Scale(NormFactor);
//if (Tf_autoTune)
// tuneTf(torque);
Vector3d invTf = TfV.Invert();
fc.pid.Kd = kdFactor * invTf;
fc.pid.Kp = (1 / (kpFactor * Math.Sqrt(2))) * fc.pid.Kd;
fc.pid.Kp.Scale(invTf);
fc.pid.Ki = (1 / (kiFactor * Math.Sqrt(2))) * fc.pid.Kp;
fc.pid.Ki.Scale(invTf);
fc.pid.intAccum = fc.pid.intAccum.Clamp(-5, 5);
// angular velocity limit:
var Wlimit = new Vector3d(Math.Sqrt(NormFactor.x * Math.PI * kWlimit),
Math.Sqrt(NormFactor.y * Math.PI * kWlimit),
Math.Sqrt(NormFactor.z * Math.PI * kWlimit));
Vector3d pidAction = fc.pid.Compute(err, omega, Wlimit);
// deadband
pidAction.x = Math.Abs(pidAction.x) >= deadband ? pidAction.x : 0.0;
pidAction.y = Math.Abs(pidAction.y) >= deadband ? pidAction.y : 0.0;
pidAction.z = Math.Abs(pidAction.z) >= deadband ? pidAction.z : 0.0;
// low pass filter, wf = 1/Tf:
Vector3d act = fc.lastAct;
act.x += (pidAction.x - fc.lastAct.x) * (1.0 / ((TfV.x / TimeWarp.fixedDeltaTime) + 1.0));
act.y += (pidAction.y - fc.lastAct.y) * (1.0 / ((TfV.y / TimeWarp.fixedDeltaTime) + 1.0));
act.z += (pidAction.z - fc.lastAct.z) * (1.0 / ((TfV.z / TimeWarp.fixedDeltaTime) + 1.0));
fc.lastAct = act;
// end MechJeb import
//---------------------------------------
float precision = Mathf.Clamp((float)(torque.x * 20f / momentOfInertia.magnitude), 0.5f, 10f);
float driveLimit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision));
act.x = Mathf.Clamp((float)act.x, -driveLimit, driveLimit);
act.y = Mathf.Clamp((float)act.y, -driveLimit, driveLimit);
act.z = Mathf.Clamp((float)act.z, -driveLimit, driveLimit);
c.roll = Mathf.Clamp((float)(c.roll + act.z), -driveLimit, driveLimit);
c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -driveLimit, driveLimit);
c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -driveLimit, driveLimit);
}
