public static string ToStringDMS(double latitude, double longitude, bool newline = false)
{
double clampedLongitude = MuUtils.ClampDegrees180(longitude);
return(AngleToDMS(latitude) + (latitude > 0 ? " N" : " S") + (newline ? "\n" : ", ")
+ AngleToDMS(clampedLongitude) + (clampedLongitude > 0 ? " E" : " W"));
}
public override void Drive(FlightCtrlState s)
{
if (controlHeading)
{
if (heading != headingLast)
{
headingPID.Reset();
headingLast = heading;
}
double instantaneousHeading = vesselState.rotationVesselSurface.eulerAngles.y;
headingErr = MuUtils.ClampDegrees180(instantaneousHeading - heading);
if (s.wheelSteer == s.wheelSteerTrim)
{
double act = headingPID.Compute(headingErr);
s.wheelSteer = Mathf.Clamp((float)act, -1, 1);
}
}
if (controlSpeed)
{
if (speed != speedLast)
{
speedPID.Reset();
speedLast = speed;
}
speedErr = speed - Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.forward);
if (s.wheelThrottle == s.wheelThrottleTrim)
{
double act = speedPID.Compute(speedErr);
s.wheelThrottle = Mathf.Clamp((float)act, -1, 1);
}
}
}
//Computes the dV of a Hohmann transfer burn at time UT that will put the apoapsis or periapsis
//of the transfer orbit on top of the target orbit.
//The output value apsisPhaseAngle is the phase angle between the transferring vessel and the
//target object as the transferring vessel crosses the target orbit at the apoapsis or periapsis
//of the transfer orbit.
//Actually, it's not exactly the phase angle. It's a sort of mean anomaly phase angle. The
//difference is not important for how this function is used by DeltaVAndTimeForHohmannTransfer.
private static Vector3d DeltaVAndApsisPhaseAngleOfHohmannTransfer(Orbit o, Orbit target, double UT, out double apsisPhaseAngle)
{
Vector3d apsisDirection = -o.SwappedRelativePositionAtUT(UT);
double desiredApsis = target.RadiusAtTrueAnomaly(target.TrueAnomalyFromVector(apsisDirection));
Vector3d dV;
if (desiredApsis > o.ApR)
{
dV = DeltaVToChangeApoapsis(o, UT, desiredApsis);
Orbit transferOrbit = o.PerturbedOrbit(UT, dV);
double transferApTime = transferOrbit.NextApoapsisTime(UT);
Vector3d transferApDirection = transferOrbit.SwappedRelativePositionAtApoapsis(); // getRelativePositionAtUT was returning NaNs! :(((((
double targetTrueAnomaly = target.TrueAnomalyFromVector(transferApDirection);
double meanAnomalyOffset = 360 * (target.TimeOfTrueAnomaly(targetTrueAnomaly, UT) - transferApTime) / target.period;
apsisPhaseAngle = meanAnomalyOffset;
}
else
{
dV = DeltaVToChangePeriapsis(o, UT, desiredApsis);
Orbit transferOrbit = o.PerturbedOrbit(UT, dV);
double transferPeTime = transferOrbit.NextPeriapsisTime(UT);
Vector3d transferPeDirection = transferOrbit.SwappedRelativePositionAtPeriapsis(); // getRelativePositionAtUT was returning NaNs! :(((((
double targetTrueAnomaly = target.TrueAnomalyFromVector(transferPeDirection);
double meanAnomalyOffset = 360 * (target.TimeOfTrueAnomaly(targetTrueAnomaly, UT) - transferPeTime) / target.period;
apsisPhaseAngle = meanAnomalyOffset;
}
apsisPhaseAngle = MuUtils.ClampDegrees180(apsisPhaseAngle);
return(dV);
}
//Computes the heading of the ground track of an orbit with a given inclination at a given latitude.
//Both inputs are in degrees.
//Convention: At equator, inclination 0 => heading 90 (east)
// inclination 90 => heading 0 (north)
// inclination -90 => heading 180 (south)
// inclination ±180 => heading 270 (west)
//Returned heading is in degrees and in the range 0 to 360.
//If the given latitude is too large, so that an orbit with a given inclination never attains the
//given latitude, then this function returns either 90 (if -90 < inclination < 90) or 270.
public static double HeadingForInclination(double inclinationDegrees, double latitudeDegrees)
{
double cosDesiredSurfaceAngle = Math.Cos(inclinationDegrees * Math.PI / 180) / Math.Cos(latitudeDegrees * Math.PI / 180);
if (Math.Abs(cosDesiredSurfaceAngle) > 1.0)
{
//If inclination < latitude, we get this case: the desired inclination is impossible
if (Math.Abs(MuUtils.ClampDegrees180(inclinationDegrees)) < 90)
{
return(90);
}
else
{
return(270);
}
}
else
{
double angleFromEast = (180 / Math.PI) * Math.Acos(cosDesiredSurfaceAngle); //an angle between 0 and 180
if (inclinationDegrees < 0)
{
angleFromEast *= -1;
}
//now angleFromEast is between -180 and 180
return(MuUtils.ClampDegrees360(90 - angleFromEast));
}
}
public static string ToStringDecimal(double latitude, double longitude, bool newline = false, int precision = 3)
{
double clampedLongitude = MuUtils.ClampDegrees180(longitude);
return(latitude.ToString("F" + precision) + "° " + (latitude > 0 ? "N" : "S") + (newline ? "\n" : ", ")
+ clampedLongitude.ToString("F" + precision) + "° " + (clampedLongitude > 0 ? "E" : "W"));
}
private double computeYaw()
{
// probably not perfect, especially when the aircraft is turning
double path = vesselState.HeadingFromDirection(vesselState.surfaceVelocity);
double nose = vesselState.HeadingFromDirection(vesselState.forward);
double angle = MuUtils.ClampDegrees180(nose - path);
return(angle);
}
public EditableAngle(double angle)
{
angle = MuUtils.ClampDegrees180(angle);
negative = (angle < 0);
angle = Math.Abs(angle);
degrees = (int)angle;
angle -= degrees;
minutes = (int)(60 * angle);
angle -= minutes / 60;
seconds = Math.Round(3600 * angle);
}
public static Coordinates GetMouseCoordinates(CelestialBody body)
{
Ray mouseRay = PlanetariumCamera.Camera.ScreenPointToRay(Input.mousePosition);
mouseRay.origin = ScaledSpace.ScaledToLocalSpace(mouseRay.origin);
Vector3d relOrigin = mouseRay.origin - body.position;
Vector3d relSurfacePosition;
double curRadius = body.pqsController.radiusMax;
double lastRadius = 0;
double error = 0;
int loops = 0;
float st = Time.time;
while (loops < 50)
{
if (PQS.LineSphereIntersection(relOrigin, mouseRay.direction, curRadius, out relSurfacePosition))
{
Vector3d surfacePoint = body.position + relSurfacePosition;
double alt = body.pqsController.GetSurfaceHeight(QuaternionD.AngleAxis(body.GetLongitude(surfacePoint), Vector3d.down) * QuaternionD.AngleAxis(body.GetLatitude(surfacePoint), Vector3d.forward) * Vector3d.right);
error = Math.Abs(curRadius - alt);
if (error < (body.pqsController.radiusMax - body.pqsController.radiusMin) / 100)
{
return(new Coordinates(body.GetLatitude(surfacePoint), MuUtils.ClampDegrees180(body.GetLongitude(surfacePoint))));
}
else
{
lastRadius = curRadius;
curRadius = alt;
loops++;
}
}
else
{
if (loops == 0)
{
break;
}
else
{ // Went too low, needs to try higher
curRadius = (lastRadius * 9 + curRadius) / 10;
loops++;
}
}
}
return(null);
}
//Vector3d must be either a position RELATIVE to referenceBody, or a velocity
public AbsoluteVector ToAbsolute(Vector3d vector3d, double UT)
{
AbsoluteVector absolute = new AbsoluteVector();
absolute.latitude = 180 / Math.PI * Math.Asin(Vector3d.Dot(vector3d.normalized, lat90AtStart));
double longitude = 180 / Math.PI * Math.Atan2(Vector3d.Dot(vector3d.normalized, lat0lon90AtStart), Vector3d.Dot(vector3d.normalized, lat0lon0AtStart));
longitude -= 360 * (UT - epoch) / referenceBody.rotationPeriod;
absolute.longitude = MuUtils.ClampDegrees180(longitude);
absolute.radius = vector3d.magnitude;
absolute.UT = UT;
return(absolute);
}
public static Coordinates GetMouseCoordinates(CelestialBody body)
{
Ray mouseRay = PlanetariumCamera.Camera.ScreenPointToRay(Input.mousePosition);
mouseRay.origin = ScaledSpace.ScaledToLocalSpace(mouseRay.origin);
Vector3d relOrigin = mouseRay.origin - body.position;
Vector3d relSurfacePosition;
if (PQS.LineSphereIntersection(relOrigin, mouseRay.direction, body.Radius, out relSurfacePosition))
{
Vector3d surfacePoint = body.position + relSurfacePosition;
return(new Coordinates(body.GetLatitude(surfacePoint), MuUtils.ClampDegrees180(body.GetLongitude(surfacePoint))));
}
else
{
return(null);
}
}
//Computes the delta-V of the burn required to change an orbit's inclination to a given value
//at a given UT. If the latitude at that time is too high, so that the desired inclination
//cannot be attained, the burn returned will achieve as low an inclination as possible (namely, inclination = latitude).
//The input inclination is in degrees.
//Note that there are two orbits through each point with a given inclination. The convention used is:
// - first, clamp newInclination to the range -180, 180
// - if newInclination > 0, do the cheaper burn to set that inclination
// - if newInclination < 0, do the more expensive burn to set that inclination
public static Vector3d DeltaVToChangeInclination(Orbit o, double UT, double newInclination)
{
double latitude = o.referenceBody.GetLatitude(o.SwappedAbsolutePositionAtUT(UT));
double desiredHeading = HeadingForInclination(newInclination, latitude);
Vector3d actualHorizontalVelocity = Vector3d.Exclude(o.Up(UT), o.SwappedOrbitalVelocityAtUT(UT));
Vector3d eastComponent = actualHorizontalVelocity.magnitude * Math.Sin(Math.PI / 180 * desiredHeading) * o.East(UT);
Vector3d northComponent = actualHorizontalVelocity.magnitude * Math.Cos(Math.PI / 180 * desiredHeading) * o.North(UT);
if (Vector3d.Dot(actualHorizontalVelocity, northComponent) < 0)
{
northComponent *= -1;
}
if (MuUtils.ClampDegrees180(newInclination) < 0)
{
northComponent *= -1;
}
Vector3d desiredHorizontalVelocity = eastComponent + northComponent;
return(desiredHorizontalVelocity - actualHorizontalVelocity);
}
void AimVelocityVector(double desiredFpa, double desiredHeading)
{
//horizontal control
double velocityHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(vesselState.surfaceVelocity, vesselState.east),
Vector3d.Dot(vesselState.surfaceVelocity, vesselState.north));
double headingTurn = Mathf.Clamp((float)MuUtils.ClampDegrees180(desiredHeading - velocityHeading), -maxYaw, maxYaw);
double noseHeading = velocityHeading + headingTurn;
double noseRoll = (maxRoll / maxYaw) * headingTurn;
//vertical control
double nosePitch = desiredFpa + stableAoA + pitchCorrection;
core.attitude.attitudeTo(noseHeading, nosePitch, noseRoll, this);
double flightPathAngle = 180 / Math.PI * Math.Atan2(vesselState.speedVertical, vesselState.speedSurfaceHorizontal);
double AoA = vesselState.vesselPitch - flightPathAngle;
stableAoA = (AoAtimeConstant * stableAoA + vesselState.deltaT * AoA) / (AoAtimeConstant + vesselState.deltaT); //a sort of integral error
pitchCorrection = (pitchCorrectionTimeConstant * pitchCorrection + vesselState.deltaT * (nosePitch - vesselState.vesselPitch)) / (pitchCorrectionTimeConstant + vesselState.deltaT);
pitchCorrection = Mathf.Clamp((float)pitchCorrection, -maxPitchCorrection, maxPitchCorrection);
}
//Returns whether a has a descending node with b. This can be false
//if a is hyperbolic and the would-be descending node is within the opening
//angle of the hyperbola.
public static bool DescendingNodeExists(this Orbit a, Orbit b)
{
return(Math.Abs(MuUtils.ClampDegrees180(a.DescendingNodeTrueAnomaly(b))) <= a.MaximumTrueAnomaly());
}
public void Update(Vessel vessel)
{
if (vessel.rigidbody == null)
{
return; //if we try to update before rigidbodies exist we spam the console with NullPointerExceptions.
}
//if (vessel.packed) return;
time = Planetarium.GetUniversalTime();
deltaT = TimeWarp.fixedDeltaTime;
CoM = vessel.findWorldCenterOfMass();
up = (CoM - vessel.mainBody.position).normalized;
Rigidbody rigidBody = vessel.rootPart.rigidbody;
if (rigidBody != null)
{
rootPartPos = rigidBody.position;
}
north = Vector3d.Exclude(up, (vessel.mainBody.position + vessel.mainBody.transform.up * (float)vessel.mainBody.Radius) - CoM).normalized;
east = vessel.mainBody.getRFrmVel(CoM).normalized;
forward = vessel.GetTransform().up;
rotationSurface = Quaternion.LookRotation(north, up);
rotationVesselSurface = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * rotationSurface);
velocityVesselOrbit = vessel.orbit.GetVel();
velocityVesselOrbitUnit = velocityVesselOrbit.normalized;
velocityVesselSurface = velocityVesselOrbit - vessel.mainBody.getRFrmVel(CoM);
velocityVesselSurfaceUnit = velocityVesselSurface.normalized;
velocityMainBodySurface = rotationSurface * velocityVesselSurface;
horizontalOrbit = Vector3d.Exclude(up, velocityVesselOrbit).normalized;
horizontalSurface = Vector3d.Exclude(up, velocityVesselSurface).normalized;
angularVelocity = Quaternion.Inverse(vessel.GetTransform().rotation) * vessel.rigidbody.angularVelocity;
radialPlusSurface = Vector3d.Exclude(velocityVesselSurface, up).normalized;
radialPlus = Vector3d.Exclude(velocityVesselOrbit, up).normalized;
normalPlusSurface = -Vector3d.Cross(radialPlusSurface, velocityVesselSurfaceUnit);
normalPlus = -Vector3d.Cross(radialPlus, velocityVesselOrbitUnit);
gravityForce = FlightGlobals.getGeeForceAtPosition(CoM);
localg = gravityForce.magnitude;
speedOrbital.value = velocityVesselOrbit.magnitude;
speedSurface.value = velocityVesselSurface.magnitude;
speedVertical.value = Vector3d.Dot(velocityVesselSurface, up);
speedSurfaceHorizontal.value = (velocityVesselSurface - (speedVertical * up)).magnitude;
speedOrbitHorizontal = (velocityVesselOrbit - (speedVertical * up)).magnitude;
vesselHeading.value = rotationVesselSurface.eulerAngles.y;
vesselPitch.value = (rotationVesselSurface.eulerAngles.x > 180) ? (360.0 - rotationVesselSurface.eulerAngles.x) : -rotationVesselSurface.eulerAngles.x;
vesselRoll.value = (rotationVesselSurface.eulerAngles.z > 180) ? (rotationVesselSurface.eulerAngles.z - 360.0) : rotationVesselSurface.eulerAngles.z;
altitudeASL.value = vessel.mainBody.GetAltitude(CoM);
RaycastHit sfc;
if (Physics.Raycast(CoM, -up, out sfc, (float)altitudeASL + 10000.0F, 1 << 15))
{
altitudeTrue.value = sfc.distance;
}
else if (vessel.mainBody.pqsController != null)
{
// from here: http://kerbalspaceprogram.com/forum/index.php?topic=10324.msg161923#msg161923
altitudeTrue.value = vessel.mainBody.GetAltitude(CoM) - (vessel.mainBody.pqsController.GetSurfaceHeight(QuaternionD.AngleAxis(vessel.mainBody.GetLongitude(CoM), Vector3d.down) * QuaternionD.AngleAxis(vessel.mainBody.GetLatitude(CoM), Vector3d.forward) * Vector3d.right) - vessel.mainBody.pqsController.radius);
}
else
{
altitudeTrue.value = vessel.mainBody.GetAltitude(CoM);
}
double surfaceAltitudeASL = altitudeASL - altitudeTrue;
altitudeBottom = altitudeTrue;
foreach (Part p in vessel.parts)
{
if (p.collider != null)
{
Vector3d bottomPoint = p.collider.ClosestPointOnBounds(vessel.mainBody.position);
double partBottomAlt = vessel.mainBody.GetAltitude(bottomPoint) - surfaceAltitudeASL;
altitudeBottom = Math.Max(0, Math.Min(altitudeBottom, partBottomAlt));
}
}
double atmosphericPressure = FlightGlobals.getStaticPressure(altitudeASL, vessel.mainBody);
if (atmosphericPressure < vessel.mainBody.atmosphereMultiplier * 1e-6)
{
atmosphericPressure = 0;
}
atmosphericDensity = FlightGlobals.getAtmDensity(atmosphericPressure);
atmosphericDensityGrams = atmosphericDensity * 1000;
orbitApA.value = vessel.orbit.ApA;
orbitPeA.value = vessel.orbit.PeA;
orbitPeriod.value = vessel.orbit.period;
orbitTimeToAp.value = vessel.orbit.timeToAp;
if (vessel.orbit.eccentricity < 1)
{
orbitTimeToPe.value = vessel.orbit.timeToPe;
}
else
{
orbitTimeToPe.value = -vessel.orbit.meanAnomaly / (2 * Math.PI / vessel.orbit.period);
}
orbitLAN.value = vessel.orbit.LAN;
orbitArgumentOfPeriapsis.value = vessel.orbit.argumentOfPeriapsis;
orbitInclination.value = vessel.orbit.inclination;
orbitEccentricity.value = vessel.orbit.eccentricity;
orbitSemiMajorAxis.value = vessel.orbit.semiMajorAxis;
latitude.value = vessel.mainBody.GetLatitude(CoM);
longitude.value = MuUtils.ClampDegrees180(vessel.mainBody.GetLongitude(CoM));
if (vessel.mainBody != Planetarium.fetch.Sun)
{
Vector3d delta = vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() + 1) - vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() - 1);
Vector3d plUp = Vector3d.Cross(vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime()) - vessel.mainBody.referenceBody.getPositionAtUT(Planetarium.GetUniversalTime()), vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() + vessel.mainBody.orbit.period / 4) - vessel.mainBody.referenceBody.getPositionAtUT(Planetarium.GetUniversalTime() + vessel.mainBody.orbit.period / 4)).normalized;
angleToPrograde = MuUtils.ClampDegrees360((((vessel.orbit.inclination > 90) || (vessel.orbit.inclination < -90)) ? 1 : -1) * ((Vector3)up).AngleInPlane(plUp, delta));
}
else
{
angleToPrograde = 0;
}
mainBody = vessel.mainBody;
radius = (CoM - vessel.mainBody.position).magnitude;
mass = thrustAvailable = thrustMinimum = massDrag = torqueRAvailable = torquePYAvailable = torqueThrustPYAvailable = 0;
rcsThrustAvailable = new Vector6();
rcsTorqueAvailable = new Vector6();
EngineInfo einfo = new EngineInfo(forward, CoM);
IntakeInfo iinfo = new IntakeInfo();
var rcsbal = vessel.GetMasterMechJeb().rcsbal;
if (vessel.ActionGroups[KSPActionGroup.RCS] && rcsbal.enabled)
{
Vector3d rot = Vector3d.zero;
foreach (Vector6.Direction dir6 in Enum.GetValues(typeof(Vector6.Direction)))
{
Vector3d dir = Vector6.directions[dir6];
double[] throttles;
List <RCSSolver.Thruster> thrusters;
rcsbal.GetThrottles(dir, out throttles, out thrusters);
if (throttles != null)
{
for (int i = 0; i < throttles.Length; i++)
{
if (throttles[i] > 0)
{
Vector3d force = thrusters[i].GetThrust(dir, rot);
rcsThrustAvailable.Add(dir * Vector3d.Dot(force * throttles[i], dir));
}
}
}
}
}
foreach (Part p in vessel.parts)
{
if (p.physicalSignificance != Part.PhysicalSignificance.NONE)
{
double partMass = p.TotalMass();
mass += partMass;
massDrag += partMass * p.maximum_drag;
}
if (vessel.ActionGroups[KSPActionGroup.RCS] && !rcsbal.enabled)
{
foreach (ModuleRCS pm in p.Modules.OfType <ModuleRCS>())
{
double maxT = pm.thrusterPower;
if ((pm.isEnabled) && (!pm.isJustForShow))
{
torqueRAvailable += maxT;
if (p.Rigidbody != null)
{
torquePYAvailable += maxT * (p.Rigidbody.worldCenterOfMass - CoM).magnitude;
}
foreach (Transform t in pm.thrusterTransforms)
{
rcsThrustAvailable.Add(-t.up * pm.thrusterPower);
}
}
}
}
if (p is CommandPod)
{
torqueRAvailable += Math.Abs(((CommandPod)p).rotPower);
torquePYAvailable += Math.Abs(((CommandPod)p).rotPower);
}
foreach (PartModule pm in p.Modules)
{
if (!pm.isEnabled)
{
continue;
}
if (pm is ModuleEngines)
{
einfo.AddNewEngine(pm as ModuleEngines);
}
else if (pm is ModuleResourceIntake)
{
iinfo.addIntake(pm as ModuleResourceIntake);
}
}
}
thrustAvailable += einfo.thrustAvailable;
thrustMinimum += einfo.thrustMinimum;
torqueThrustPYAvailable += einfo.torqueThrustPYAvailable;
// Convert the resource information from the einfo and iinfo format
// to the more useful ResourceInfo format.
resources = new Dictionary <int, ResourceInfo>();
foreach (var info in einfo.resourceRequired)
{
int id = info.Key;
var req = info.Value;
resources[id] = new ResourceInfo(
PartResourceLibrary.Instance.GetDefinition(id),
req.requiredLastFrame,
req.requiredAtMaxThrottle,
iinfo.getIntakes(id));
}
int intakeAirId = PartResourceLibrary.Instance.GetDefinition("IntakeAir").id;
intakeAir = 0;
intakeAirNeeded = 0;
intakeAirAtMax = 0;
if (resources.ContainsKey(intakeAirId))
{
intakeAir = resources[intakeAirId].intakeProvided;
intakeAirNeeded = resources[intakeAirId].required;
intakeAirAtMax = resources[intakeAirId].requiredAtMaxThrottle;
}
angularMomentum = new Vector3d(angularVelocity.x * MoI.x, angularVelocity.y * MoI.y, angularVelocity.z * MoI.z);
maxThrustAccel = thrustAvailable / mass;
minThrustAccel = thrustMinimum / mass;
inertiaTensor = new Matrix3x3();
foreach (Part p in vessel.parts)
{
if (p.Rigidbody == null)
{
continue;
}
//Compute the contributions to the vessel inertia tensor due to the part inertia tensor
Vector3d principalMoments = p.Rigidbody.inertiaTensor;
Quaternion princAxesRot = Quaternion.Inverse(vessel.GetTransform().rotation) * p.transform.rotation * p.Rigidbody.inertiaTensorRotation;
Quaternion invPrincAxesRot = Quaternion.Inverse(princAxesRot);
for (int i = 0; i < 3; i++)
{
Vector3d iHat = Vector3d.zero;
iHat[i] = 1;
for (int j = 0; j < 3; j++)
{
Vector3d jHat = Vector3d.zero;
jHat[j] = 1;
inertiaTensor[i, j] += Vector3d.Dot(iHat, princAxesRot * Vector3d.Scale(principalMoments, invPrincAxesRot * jHat));
}
}
//Compute the contributions to the vessel inertia tensor due to the part mass and position
double partMass = p.TotalMass();
Vector3 partPosition = vessel.transform.InverseTransformDirection(p.Rigidbody.worldCenterOfMass - CoM);
for (int i = 0; i < 3; i++)
{
inertiaTensor[i, i] += partMass * partPosition.sqrMagnitude;
for (int j = 0; j < 3; j++)
{
inertiaTensor[i, j] += -partMass * partPosition[i] * partPosition[j];
}
}
}
MoI = new Vector3d(inertiaTensor[0, 0], inertiaTensor[1, 1], inertiaTensor[2, 2]);
angularMomentum = inertiaTensor * angularVelocity;
}
public void Update(Vessel vessel)
{
if (vessel.rigidbody == null)
{
return; //if we try to update before rigidbodies exist we spam the console with NullPointerExceptions.
}
//if (vessel.packed) return;
// To investigate some strange error
if ((vessel.mainBody == null || (object)(vessel.mainBody) == null) && counter == 0)
{
if ((object)(vessel.mainBody) == null)
{
MechJebCore.print("vessel.mainBody is proper null");
}
else
{
MechJebCore.print("vessel.mainBody is Unity null");
}
counter = counter++ % 100;
}
time = Planetarium.GetUniversalTime();
deltaT = TimeWarp.fixedDeltaTime;
CoM = vessel.findWorldCenterOfMass();
up = (CoM - vessel.mainBody.position).normalized;
Rigidbody rigidBody = vessel.rootPart.rigidbody;
if (rigidBody != null)
{
rootPartPos = rigidBody.position;
}
north = Vector3d.Exclude(up, (vessel.mainBody.position + vessel.mainBody.transform.up * (float)vessel.mainBody.Radius) - CoM).normalized;
east = vessel.mainBody.getRFrmVel(CoM).normalized;
forward = vessel.GetTransform().up;
rotationSurface = Quaternion.LookRotation(north, up);
rotationVesselSurface = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * rotationSurface);
// velocityVesselOrbit = vessel.orbit.GetVel();
// velocityVesselOrbitUnit = velocityVesselOrbit.normalized;
// velocityVesselSurface = velocityVesselOrbit - vessel.mainBody.getRFrmVel(CoM);
// velocityVesselSurfaceUnit = velocityVesselSurface.normalized;
velocityMainBodySurface = rotationSurface * vessel.srf_velocity;
horizontalOrbit = Vector3d.Exclude(up, vessel.obt_velocity).normalized;
horizontalSurface = Vector3d.Exclude(up, vessel.srf_velocity).normalized;
angularVelocity = Quaternion.Inverse(vessel.GetTransform().rotation) * vessel.rigidbody.angularVelocity;
radialPlusSurface = Vector3d.Exclude(vessel.srf_velocity, up).normalized;
radialPlus = Vector3d.Exclude(vessel.obt_velocity, up).normalized;
normalPlusSurface = -Vector3d.Cross(radialPlusSurface, vessel.srf_velocity.normalized);
normalPlus = -Vector3d.Cross(radialPlus, vessel.obt_velocity.normalized);
gravityForce = FlightGlobals.getGeeForceAtPosition(CoM);
localg = gravityForce.magnitude;
speedOrbital.value = vessel.obt_velocity.magnitude;
speedSurface.value = vessel.srf_velocity.magnitude;
speedVertical.value = Vector3d.Dot(vessel.srf_velocity, up);
speedSurfaceHorizontal.value = Vector3d.Exclude(up, vessel.srf_velocity).magnitude; //(velocityVesselSurface - (speedVertical * up)).magnitude;
speedOrbitHorizontal = (vessel.obt_velocity - (speedVertical * up)).magnitude;
vesselHeading.value = rotationVesselSurface.eulerAngles.y;
vesselPitch.value = (rotationVesselSurface.eulerAngles.x > 180) ? (360.0 - rotationVesselSurface.eulerAngles.x) : -rotationVesselSurface.eulerAngles.x;
vesselRoll.value = (rotationVesselSurface.eulerAngles.z > 180) ? (rotationVesselSurface.eulerAngles.z - 360.0) : rotationVesselSurface.eulerAngles.z;
altitudeASL.value = vessel.mainBody.GetAltitude(CoM);
//RaycastHit sfc;
//if (Physics.Raycast(CoM, -up, out sfc, (float)altitudeASL + 10000.0F, 1 << 15))
//{
// altitudeTrue.value = sfc.distance;
//}
//else if (vessel.mainBody.pqsController != null)
//{
// // from here: http://kerbalspaceprogram.com/forum/index.php?topic=10324.msg161923#msg161923
// altitudeTrue.value = vessel.mainBody.GetAltitude(CoM) - (vessel.mainBody.pqsController.GetSurfaceHeight(QuaternionD.AngleAxis(vessel.mainBody.GetLongitude(CoM), Vector3d.down) * QuaternionD.AngleAxis(vessel.mainBody.GetLatitude(CoM), Vector3d.forward) * Vector3d.right) - vessel.mainBody.pqsController.radius);
//}
//else
//{
// altitudeTrue.value = vessel.mainBody.GetAltitude(CoM);
//}
//double surfaceAltitudeASL = altitudeASL - altitudeTrue;
double surfaceAltitudeASL = vessel.mainBody.pqsController != null ? vessel.pqsAltitude : 0d;
altitudeTrue.value = altitudeASL - surfaceAltitudeASL;
altitudeBottom = altitudeTrue;
foreach (Part p in vessel.parts)
{
if (p.collider != null)
{
Vector3d bottomPoint = p.collider.ClosestPointOnBounds(vessel.mainBody.position);
double partBottomAlt = vessel.mainBody.GetAltitude(bottomPoint) - surfaceAltitudeASL;
altitudeBottom = Math.Max(0, Math.Min(altitudeBottom, partBottomAlt));
}
}
double atmosphericPressure = FlightGlobals.getStaticPressure(altitudeASL, vessel.mainBody);
if (atmosphericPressure < vessel.mainBody.atmosphereMultiplier * 1e-6)
{
atmosphericPressure = 0;
}
atmosphericDensity = FlightGlobals.getAtmDensity(atmosphericPressure);
atmosphericDensityGrams = atmosphericDensity * 1000;
orbitApA.value = vessel.orbit.ApA;
orbitPeA.value = vessel.orbit.PeA;
orbitPeriod.value = vessel.orbit.period;
orbitTimeToAp.value = vessel.orbit.timeToAp;
if (vessel.orbit.eccentricity < 1)
{
orbitTimeToPe.value = vessel.orbit.timeToPe;
}
else
{
orbitTimeToPe.value = -vessel.orbit.meanAnomaly / (2 * Math.PI / vessel.orbit.period);
}
orbitLAN.value = vessel.orbit.LAN;
orbitArgumentOfPeriapsis.value = vessel.orbit.argumentOfPeriapsis;
orbitInclination.value = vessel.orbit.inclination;
orbitEccentricity.value = vessel.orbit.eccentricity;
orbitSemiMajorAxis.value = vessel.orbit.semiMajorAxis;
latitude.value = vessel.mainBody.GetLatitude(CoM);
longitude.value = MuUtils.ClampDegrees180(vessel.mainBody.GetLongitude(CoM));
if (vessel.mainBody != Planetarium.fetch.Sun)
{
Vector3d delta = vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() + 1) - vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() - 1);
Vector3d plUp = Vector3d.Cross(vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime()) - vessel.mainBody.referenceBody.getPositionAtUT(Planetarium.GetUniversalTime()), vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() + vessel.mainBody.orbit.period / 4) - vessel.mainBody.referenceBody.getPositionAtUT(Planetarium.GetUniversalTime() + vessel.mainBody.orbit.period / 4)).normalized;
angleToPrograde = MuUtils.ClampDegrees360((((vessel.orbit.inclination > 90) || (vessel.orbit.inclination < -90)) ? 1 : -1) * ((Vector3)up).AngleInPlane(plUp, delta));
}
else
{
angleToPrograde = 0;
}
mainBody = vessel.mainBody;
radius = (CoM - vessel.mainBody.position).magnitude;
mass = massDrag = torqueThrustPYAvailable = 0;
thrustVectorLastFrame = new Vector3d();
thrustVectorMaxThrottle = new Vector3d();
thrustVectorMinThrottle = new Vector3d();
torqueAvailable = new Vector3d();
rcsThrustAvailable = new Vector6();
rcsTorqueAvailable = new Vector6();
ctrlTorqueAvailable = new Vector6();
EngineInfo einfo = new EngineInfo(CoM);
IntakeInfo iinfo = new IntakeInfo();
parachutes = new List <ModuleParachute>();
var rcsbal = vessel.GetMasterMechJeb().rcsbal;
if (vessel.ActionGroups[KSPActionGroup.RCS] && rcsbal.enabled)
{
Vector3d rot = Vector3d.zero;
foreach (Vector6.Direction dir6 in Enum.GetValues(typeof(Vector6.Direction)))
{
Vector3d dir = Vector6.directions[dir6];
double[] throttles;
List <RCSSolver.Thruster> thrusters;
rcsbal.GetThrottles(dir, out throttles, out thrusters);
if (throttles != null)
{
for (int i = 0; i < throttles.Length; i++)
{
if (throttles[i] > 0)
{
Vector3d force = thrusters[i].GetThrust(dir, rot);
rcsThrustAvailable.Add(vessel.GetTransform().InverseTransformDirection(dir * Vector3d.Dot(force * throttles[i], dir)));
}
}
}
}
}
hasMFE = false;
foreach (Part p in vessel.parts)
{
if (p.IsPhysicallySignificant())
{
double partMass = p.TotalMass();
mass += partMass;
massDrag += partMass * p.maximum_drag;
}
if (vessel.ActionGroups[KSPActionGroup.RCS] && !rcsbal.enabled)
{
foreach (ModuleRCS pm in p.Modules.OfType <ModuleRCS>())
{
double maxT = pm.thrusterPower;
Vector3d partPosition = p.Rigidbody.worldCenterOfMass - CoM;
if ((pm.isEnabled) && (!pm.isJustForShow))
{
foreach (Transform t in pm.thrusterTransforms)
{
Vector3d thrusterThrust = vessel.GetTransform().InverseTransformDirection(-t.up.normalized) * pm.thrusterPower;
rcsThrustAvailable.Add(thrusterThrust);
Vector3d thrusterTorque = Vector3.Cross(vessel.GetTransform().InverseTransformDirection(partPosition), thrusterThrust);
rcsTorqueAvailable.Add(thrusterTorque);
}
}
}
}
if (p is ControlSurface)
{
Vector3d partPosition = p.Rigidbody.worldCenterOfMass - CoM;
ControlSurface cs = (p as ControlSurface);
Vector3d airSpeed = vessel.srf_velocity + Vector3.Cross(cs.Rigidbody.angularVelocity, cs.transform.position - cs.Rigidbody.position);
// Air Speed is velocityVesselSurface
// AddForceAtPosition seems to need the airspeed vector rotated with the flap rotation x its surface
Quaternion airSpeedRot = Quaternion.AngleAxis(cs.ctrlSurfaceRange * cs.ctrlSurfaceArea, cs.transform.rotation * cs.pivotAxis);
Vector3 ctrlTroquePos = vessel.GetTransform().InverseTransformDirection(Vector3.Cross(partPosition, cs.getLiftVector(airSpeedRot * airSpeed)));
Vector3 ctrlTroqueNeg = vessel.GetTransform().InverseTransformDirection(Vector3.Cross(partPosition, cs.getLiftVector(Quaternion.Inverse(airSpeedRot) * airSpeed)));
ctrlTorqueAvailable.Add(ctrlTroquePos);
ctrlTorqueAvailable.Add(ctrlTroqueNeg);
}
if (p is CommandPod)
{
torqueAvailable += Vector3d.one * Math.Abs(((CommandPod)p).rotPower);
}
foreach (VesselStatePartExtension vspe in vesselStatePartExtensions)
{
vspe(p);
}
foreach (PartModule pm in p.Modules)
{
if (!pm.isEnabled)
{
continue;
}
if (pm is ModuleReactionWheel)
{
ModuleReactionWheel rw = (ModuleReactionWheel)pm;
// I had to remove the test for active in .23 since the new ressource system reply to the RW that
// there is no energy available when the RW do tiny adjustement.
// I replaceed it with a test that check if there is electricity anywhere on the ship.
// Let's hope we don't get reaction wheel that use something else
//if (rw.wheelState == ModuleReactionWheel.WheelState.Active && !rw.stateString.Contains("Not enough"))
if (rw.wheelState == ModuleReactionWheel.WheelState.Active && vessel.HasElectricCharge())
{
torqueAvailable += new Vector3d(rw.PitchTorque, rw.RollTorque, rw.YawTorque);
}
}
else if (pm is ModuleEngines)
{
einfo.AddNewEngine(pm as ModuleEngines);
}
else if (pm is ModuleEnginesFX)
{
einfo.AddNewEngine(pm as ModuleEnginesFX);
}
else if (pm is ModuleResourceIntake)
{
iinfo.addIntake(pm as ModuleResourceIntake);
}
else if (pm is ModuleParachute)
{
parachutes.Add(pm as ModuleParachute);
}
else if (pm is ModuleControlSurface)
{
// TODO : Tweakable for ignorePitch / ignoreYaw / ignoreRoll
ModuleControlSurface cs = (pm as ModuleControlSurface);
Vector3d partPosition = p.Rigidbody.worldCenterOfMass - CoM;
Vector3d airSpeed = vessel.srf_velocity + Vector3.Cross(cs.part.Rigidbody.angularVelocity, cs.transform.position - cs.part.Rigidbody.position);
Quaternion airSpeedRot = Quaternion.AngleAxis(cs.ctrlSurfaceRange * cs.ctrlSurfaceArea, cs.transform.rotation * Vector3.right);
Vector3 ctrlTroquePos = vessel.GetTransform().InverseTransformDirection(Vector3.Cross(partPosition, cs.getLiftVector(airSpeedRot * airSpeed)));
Vector3 ctrlTroqueNeg = vessel.GetTransform().InverseTransformDirection(Vector3.Cross(partPosition, cs.getLiftVector(Quaternion.Inverse(airSpeedRot) * airSpeed)));
ctrlTorqueAvailable.Add(ctrlTroquePos);
ctrlTorqueAvailable.Add(ctrlTroqueNeg);
}
if (pm.ClassName == "ModuleEngineConfigs" || pm.ClassName == "ModuleHybridEngine" || pm.ClassName == "ModuleHybridEngines")
{
hasMFE = true;
}
foreach (VesselStatePartModuleExtension vspme in vesselStatePartModuleExtensions)
{
vspme(pm);
}
}
}
// Consider all the parachutes
{
bool tempParachuteDeployed = false;
foreach (ModuleParachute p in parachutes)
{
if (p.deploymentState == ModuleParachute.deploymentStates.DEPLOYED || p.deploymentState == ModuleParachute.deploymentStates.SEMIDEPLOYED)
{
tempParachuteDeployed = true;
break;
}
}
this.parachuteDeployed = tempParachuteDeployed;
}
torqueAvailable += Vector3d.Max(rcsTorqueAvailable.positive, rcsTorqueAvailable.negative); // Should we use Max or Min ?
torqueAvailable += Vector3d.Max(ctrlTorqueAvailable.positive, ctrlTorqueAvailable.negative); // Should we use Max or Min ?
thrustVectorMaxThrottle += einfo.thrustMax;
thrustVectorMinThrottle += einfo.thrustMin;
thrustVectorLastFrame += einfo.thrustCurrent;
torqueThrustPYAvailable += einfo.torqueThrustPYAvailable;
if (thrustVectorMaxThrottle.magnitude == 0 && vessel.ActionGroups[KSPActionGroup.RCS])
{
rcsThrust = true;
thrustVectorMaxThrottle += (Vector3d)(vessel.transform.up) * rcsThrustAvailable.down;
}
else
{
rcsThrust = false;
}
// Convert the resource information from the einfo and iinfo format
// to the more useful ResourceInfo format.
resources = new Dictionary <int, ResourceInfo>();
foreach (var info in einfo.resourceRequired)
{
int id = info.Key;
var req = info.Value;
resources[id] = new ResourceInfo(
PartResourceLibrary.Instance.GetDefinition(id),
req.requiredLastFrame,
req.requiredAtMaxThrottle,
iinfo.getIntakes(id));
}
int intakeAirId = PartResourceLibrary.Instance.GetDefinition("IntakeAir").id;
intakeAir = 0;
intakeAirNeeded = 0;
intakeAirAtMax = 0;
intakeAirAllIntakes = 0;
if (resources.ContainsKey(intakeAirId))
{
intakeAir = resources[intakeAirId].intakeProvided;
intakeAirAllIntakes = resources[intakeAirId].intakeAvailable;
intakeAirNeeded = resources[intakeAirId].required;
intakeAirAtMax = resources[intakeAirId].requiredAtMaxThrottle;
}
angularMomentum = new Vector3d(angularVelocity.x * MoI.x, angularVelocity.y * MoI.y, angularVelocity.z * MoI.z);
inertiaTensor = new Matrix3x3();
foreach (Part p in vessel.parts)
{
if (p.Rigidbody == null)
{
continue;
}
//Compute the contributions to the vessel inertia tensor due to the part inertia tensor
Vector3d principalMoments = p.Rigidbody.inertiaTensor;
Quaternion princAxesRot = Quaternion.Inverse(vessel.GetTransform().rotation) * p.transform.rotation * p.Rigidbody.inertiaTensorRotation;
Quaternion invPrincAxesRot = Quaternion.Inverse(princAxesRot);
for (int i = 0; i < 3; i++)
{
Vector3d iHat = Vector3d.zero;
iHat[i] = 1;
for (int j = 0; j < 3; j++)
{
Vector3d jHat = Vector3d.zero;
jHat[j] = 1;
inertiaTensor[i, j] += Vector3d.Dot(iHat, princAxesRot * Vector3d.Scale(principalMoments, invPrincAxesRot * jHat));
}
}
//Compute the contributions to the vessel inertia tensor due to the part mass and position
double partMass = p.TotalMass();
Vector3 partPosition = vessel.GetTransform().InverseTransformDirection(p.Rigidbody.worldCenterOfMass - CoM);
for (int i = 0; i < 3; i++)
{
inertiaTensor[i, i] += partMass * partPosition.sqrMagnitude;
for (int j = 0; j < 3; j++)
{
inertiaTensor[i, j] += -partMass * partPosition[i] * partPosition[j];
}
}
}
MoI = new Vector3d(inertiaTensor[0, 0], inertiaTensor[1, 1], inertiaTensor[2, 2]);
angularMomentum = inertiaTensor * angularVelocity;
}
public override void Drive(FlightCtrlState s)
{
UpdatePID();
//SpeedHold (set AccelerationTarget automatically to hold speed)
if (SpeedHoldEnabled)
{
double spd = vesselState.speedSurface;
cur_acc = (spd - _spd) / Time.fixedDeltaTime;
_spd = spd;
RealAccelerationTarget = (SpeedTarget - spd) / 4;
a_err = (RealAccelerationTarget - cur_acc);
AccelerationPIDController.intAccum = MuUtils.Clamp(AccelerationPIDController.intAccum, -1 / AccKi, 1 / AccKi);
double t_act = AccelerationPIDController.Compute(a_err);
if (!double.IsNaN(t_act))
{
core.thrust.targetThrottle = (float)MuUtils.Clamp(t_act, 0, 1);
}
else
{
core.thrust.targetThrottle = 0.0f;
AccelerationPIDController.Reset();
}
}
//AltitudeHold (set VertSpeed automatically to hold altitude)
if (AltitudeHoldEnabled)
{
RealVertSpeedTarget = MuUtils.Clamp(fixVertSpeed(AltitudeTarget - vesselState.altitudeASL), -VertSpeedMax, VertSpeedMax);
}
else
{
RealVertSpeedTarget = VertSpeedTarget;
}
//VertSpeedHold
if (VertSpeedHoldEnabled)
{
double vertspd = vesselState.speedVertical;
v_err = RealVertSpeedTarget - vertspd;
VertSpeedPIDController.intAccum = MuUtils.Clamp(VertSpeedPIDController.intAccum, -1 / (VerKi * VerPIDScale), 1 / (VerKi * VerPIDScale));
double p_act = VertSpeedPIDController.Compute(v_err);
//Log.dbg(p_act);
if (double.IsNaN(p_act))
{
VertSpeedPIDController.Reset();
}
else
{
s.pitch = Mathf.Clamp((float)p_act, -1, 1);
}
}
//HeadingHold
double curHeading = vesselState.HeadingFromDirection(vesselState.forward);
if (HeadingHoldEnabled)
{
double toturn = MuUtils.ClampDegrees180(HeadingTarget - curHeading);
RealRollTarget = MuUtils.Clamp(toturn * 2, -RollMax, RollMax);
}
else
{
RealRollTarget = RollTarget;
}
if (RollHoldEnabled)
{
double roll_err = MuUtils.ClampDegrees180(vesselState.vesselRoll + RealRollTarget);
RollPIDController.intAccum = MuUtils.Clamp(RollPIDController.intAccum, -100 / AccKi, 100 / AccKi);
double roll_act = RollPIDController.Compute(roll_err);
s.roll = Mathf.Clamp((float)roll_act / 100, -1, 1);
}
if (HeadingHoldEnabled)
{
double yaw_err = MuUtils.ClampDegrees180(HeadingTarget - curHeading);
YawPIDController.intAccum = MuUtils.Clamp(YawPIDController.intAccum, -YawLimit * 100 / AccKi, YawLimit * 100 / AccKi);
double yaw_act = YawPIDController.Compute(yaw_err);
s.yaw = (float)MuUtils.Clamp(yaw_act / 100, -YawLimit, +YawLimit);
}
}
//Returns whether o has a descending node with the equator. This can be false
//if o is hyperbolic and the would-be descending node is within the opening
//angle of the hyperbola.
public static bool DescendingNodeEquatorialExists(this Orbit o)
{
return(Math.Abs(MuUtils.ClampDegrees180(o.DescendingNodeEquatorialTrueAnomaly())) <= o.MaximumTrueAnomaly());
}
public override void Drive(FlightCtrlState s) // TODO put the brake in when running out of power to prevent nighttime solar failures on hills, or atleast try to
{ // TODO make distance calculation for 'reached' determination consider the rover and waypoint on sealevel to prevent height differences from messing it up
if (orbit.referenceBody != lastBody)
{
WaypointIndex = -1; Waypoints.Clear();
}
MechJebRoverWaypoint wp = (WaypointIndex > -1 && WaypointIndex < Waypoints.Count ? Waypoints[WaypointIndex] : null);
var curSpeed = vesselState.speedSurface;
etaSpeed.value = curSpeed;
if (wp != null && wp.Body == orbit.referenceBody)
{
if (controlHeading)
{
heading = Math.Round(HeadingToPos(vessel.CoM, wp.Position), 1);
}
if (controlSpeed)
{
var nextWP = (WaypointIndex < Waypoints.Count - 1 ? Waypoints[WaypointIndex + 1] : (LoopWaypoints ? Waypoints[0] : null));
var distance = Vector3.Distance(vessel.CoM, wp.Position);
//var maxSpeed = (wp.MaxSpeed > 0 ? Math.Min((float)speed, wp.MaxSpeed) : speed); // use waypoints maxSpeed if set and smaller than set the speed or just stick with the set speed
var maxSpeed = (wp.MaxSpeed > 0 ? wp.MaxSpeed : speed); // speed used to go towards the waypoint, using the waypoints maxSpeed if set or just stick with the set speed
var minSpeed = (wp.MinSpeed > 0 ? wp.MinSpeed :
(nextWP != null ? TurningSpeed((nextWP.MaxSpeed > 0 ? nextWP.MaxSpeed : speed), heading - HeadingToPos(wp.Position, nextWP.Position)) :
(distance - wp.Radius > 50 ? turnSpeed.val : 1)));
minSpeed = (wp.Quicksave ? 0 : minSpeed);
// ^ speed used to go through the waypoint, using half the set speed or maxSpeed as minSpeed for routing waypoints (all except the last)
var brakeFactor = Math.Max((curSpeed - minSpeed) * 1, 3);
var newSpeed = Math.Min(maxSpeed, Math.Max((distance - wp.Radius) / brakeFactor, minSpeed)); // brake when getting closer
newSpeed = (newSpeed > turnSpeed ? TurningSpeed(newSpeed, headingErr) : newSpeed); // reduce speed when turning a lot
var radius = Math.Max(wp.Radius, 10 / 0.8); // alternative radius so negative radii can still make it go full speed through waypoints for navigation reasons
if (distance < radius)
{
if (WaypointIndex + 1 >= Waypoints.Count) // last waypoint
{
newSpeed = new [] { newSpeed, (distance < radius * 0.8 ? 0 : 1) }.Min();
// ^ limit speed so it'll only go from 1m/s to full stop when braking to prevent accidents on moons
if (LoopWaypoints)
{
WaypointIndex = 0;
}
else
{
newSpeed = -0.25;
tgtSpeed.force(newSpeed);
if (curSpeed < 0.85)
{
if (wp.Quicksave)
{
if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
{
WaypointIndex = -1;
controlHeading = controlSpeed = false;
QuickSaveLoad.QuickSave();
}
}
else
{
WaypointIndex = -1;
controlHeading = controlSpeed = false;
}
}
// else {
// Debug.Log("Is this even getting called?");
// WaypointIndex++;
// }
}
}
else
{
if (wp.Quicksave)
{
newSpeed = -0.25;
tgtSpeed.force(newSpeed);
if (curSpeed < 0.85)
{
if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
{
WaypointIndex++;
QuickSaveLoad.QuickSave();
}
}
}
else
{
WaypointIndex++;
}
}
}
vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, (GameSettings.BRAKES.GetKey() && vessel.isActiveVessel) || ((s.wheelThrottle == 0 || !vessel.isActiveVessel) && curSpeed < 0.85 && newSpeed < 0.85));
// ^ brake if needed to prevent rolling, hopefully
tgtSpeed.value = Math.Round(newSpeed, 1);
}
}
if (controlHeading)
{
if (heading != headingLast)
{
headingPID.Reset();
headingLast = heading;
}
double instantaneousHeading = vesselState.rotationVesselSurface.eulerAngles.y;
headingErr = MuUtils.ClampDegrees180(instantaneousHeading - heading);
if (s.wheelSteer == s.wheelSteerTrim || FlightGlobals.ActiveVessel != vessel)
{
float spd = Mathf.Min((float)speed, (float)turnSpeed); // if a slower speed than the turnspeed is used also be more careful with the steering
float limit = (curSpeed <= turnSpeed ? 1 : Mathf.Clamp((float)((spd * spd) / (curSpeed * curSpeed)), 0.2f, 1f));
double act = headingPID.Compute(headingErr);
s.wheelSteer = Mathf.Clamp((float)act, -limit, limit);
}
}
// Brake if there is no controler (Pilot eject from seat)
if (brakeOnEject && vessel.GetReferenceTransformPart() == null)
{
s.wheelThrottle = 0;
vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
}
else if (controlSpeed)
{
if (speed != speedLast)
{
speedPID.Reset();
speedLast = speed;
}
speedErr = (WaypointIndex == -1 ? speed.val : tgtSpeed.value) - Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.forward);
if (s.wheelThrottle == s.wheelThrottleTrim || FlightGlobals.ActiveVessel != vessel)
{
double act = speedPID.Compute(speedErr);
s.wheelThrottle = Mathf.Clamp((float)act, -1, 1);
}
}
}
public override void Drive(FlightCtrlState s) // TODO put the brake in when running out of power to prevent nighttime solar failures on hills, or atleast try to
{ // TODO make distance calculation for 'reached' determination consider the rover and waypoint on sealevel to prevent height differences from messing it up -- should be done now?
if (orbit.referenceBody != lastBody)
{
WaypointIndex = -1; Waypoints.Clear();
}
MechJebWaypoint wp = (WaypointIndex > -1 && WaypointIndex < Waypoints.Count ? Waypoints[WaypointIndex] : null);
var brake = vessel.ActionGroups[KSPActionGroup.Brakes]; // keep brakes locked if they are
curSpeed = Vector3d.Dot(vesselState.surfaceVelocity, vesselState.forward);
CalculateTraction();
speedIntAcc = speedPID.intAccum;
if (wp != null && wp.Body == orbit.referenceBody)
{
if (ControlHeading)
{
heading.val = Math.Round(HeadingToPos(vessel.CoM, wp.Position), 1);
}
if (ControlSpeed)
{
var nextWP = (WaypointIndex < Waypoints.Count - 1 ? Waypoints[WaypointIndex + 1] : (LoopWaypoints ? Waypoints[0] : null));
var distance = Vector3.Distance(vessel.CoM, wp.Position);
if (wp.Target != null)
{
distance += (float)(wp.Target.srfSpeed * curSpeed) / 2;
}
// var maxSpeed = (wp.MaxSpeed > 0 ? Math.Min((float)speed, wp.MaxSpeed) : speed); // use waypoints maxSpeed if set and smaller than set the speed or just stick with the set speed
var maxSpeed = (wp.MaxSpeed > 0 ? wp.MaxSpeed : speed); // speed used to go towards the waypoint, using the waypoints maxSpeed if set or just stick with the set speed
var minSpeed = (wp.MinSpeed > 0 ? wp.MinSpeed :
(nextWP != null ? TurningSpeed((nextWP.MaxSpeed > 0 ? nextWP.MaxSpeed : speed), heading - HeadingToPos(wp.Position, nextWP.Position)) :
(distance - wp.Radius > 50 ? turnSpeed.val : 1)));
minSpeed = (wp.Quicksave ? 1 : minSpeed);
// ^ speed used to go through the waypoint, using half the set speed or maxSpeed as minSpeed for routing waypoints (all except the last)
var newSpeed = Math.Min(maxSpeed, Math.Max((distance - wp.Radius) / curSpeed, minSpeed)); // brake when getting closer
newSpeed = (newSpeed > turnSpeed ? TurningSpeed(newSpeed, headingErr) : newSpeed); // reduce speed when turning a lot
var radius = Math.Max(wp.Radius, 10);
if (distance < radius)
{
if (WaypointIndex + 1 >= Waypoints.Count) // last waypoint
{
newSpeed = new [] { newSpeed, (distance < radius * 0.8 ? 0 : 1) }.Min();
// ^ limit speed so it'll only go from 1m/s to full stop when braking to prevent accidents on moons
if (LoopWaypoints)
{
WaypointIndex = 0;
}
else
{
newSpeed = 0;
brake = true;
if (curSpeed < brakeSpeedLimit)
{
if (wp.Quicksave)
{
if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
{
WaypointIndex = -1;
ControlHeading = ControlSpeed = false;
QuickSaveLoad.QuickSave();
}
}
else
{
WaypointIndex = -1;
ControlHeading = ControlSpeed = false;
}
}
}
}
else
{
if (wp.Quicksave)
{
newSpeed = 0;
if (curSpeed < brakeSpeedLimit)
{
if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
{
WaypointIndex++;
QuickSaveLoad.QuickSave();
}
}
}
else
{
WaypointIndex++;
}
}
}
brake = brake || ((s.wheelThrottle == 0 || !vessel.isActiveVessel) && curSpeed < brakeSpeedLimit && newSpeed < brakeSpeedLimit);
// ^ brake if needed to prevent rolling, hopefully
tgtSpeed = (newSpeed >= 0 ? newSpeed : 0);
}
}
if (ControlHeading)
{
headingPID.intAccum = Mathf.Clamp((float)headingPID.intAccum, -1, 1);
double instantaneousHeading = vesselState.rotationVesselSurface.eulerAngles.y;
headingErr = MuUtils.ClampDegrees180(instantaneousHeading - heading);
if (s.wheelSteer == s.wheelSteerTrim || FlightGlobals.ActiveVessel != vessel)
{
float limit = (Math.Abs(curSpeed) > turnSpeed ? Mathf.Clamp((float)((turnSpeed + 6) / Square(curSpeed)), 0.1f, 1f) : 1f);
// turnSpeed needs to be higher than curSpeed or it will never steer as much as it could even at 0.2m/s above it
double act = headingPID.Compute(headingErr);
if (traction >= tractionLimit)
{
s.wheelSteer = Mathf.Clamp((float)act, -limit, limit);
// prevents it from flying above a waypoint and landing with steering at max while still going fast
}
}
}
// Brake if there is no controler (Pilot eject from seat)
if (BrakeOnEject && vessel.GetReferenceTransformPart() == null)
{
s.wheelThrottle = 0;
brake = true;
}
else if (ControlSpeed)
{
speedPID.intAccum = Mathf.Clamp((float)speedPID.intAccum, -5, 5);
speedErr = (WaypointIndex == -1 ? speed.val : tgtSpeed) - Vector3d.Dot(vesselState.surfaceVelocity, vesselState.forward);
if (s.wheelThrottle == s.wheelThrottleTrim || FlightGlobals.ActiveVessel != vessel)
{
float act = (float)speedPID.Compute(speedErr);
s.wheelThrottle = Mathf.Clamp(act, -1f, 1f);
if (curSpeed < 0 & s.wheelThrottle < 0)
{
s.wheelThrottle = 0;
} // don't go backwards
if (Mathf.Sign(act) + Mathf.Sign(s.wheelThrottle) == 0)
{
s.wheelThrottle = Mathf.Clamp(act, -1f, 1f);
}
if (speedErr < -1 && StabilityControl && Mathf.Sign(s.wheelThrottle) + Math.Sign(curSpeed) == 0)
{
brake = true;
}
lastThrottle = Mathf.Clamp(s.wheelThrottle, -1, 1);
}
}
if (StabilityControl)
{
if (!core.attitude.users.Contains(this))
{
core.attitude.users.Add(this);
}
var fSpeed = (float)curSpeed;
Vector3 fwd = (Vector3)(traction > 0 ? // V when the speed is low go for the vessels forward, else with a bit of velocity
vesselState.forward * 4 - vessel.transform.right * s.wheelSteer * Mathf.Sign(fSpeed) : // and then add the steering
vesselState.surfaceVelocity); // in the air so follow velocity
Vector3.OrthoNormalize(ref norm, ref fwd);
var quat = Quaternion.LookRotation(fwd, norm);
if (vesselState.torqueAvailable.sqrMagnitude > 0)
{
core.attitude.attitudeTo(quat, AttitudeReference.INERTIAL, this);
}
}
if (BrakeOnEnergyDepletion)
{
var batteries = vessel.Parts.FindAll(p => p.Resources.Contains(PartResourceLibrary.ElectricityHashcode) && p.Resources.Get(PartResourceLibrary.ElectricityHashcode).flowState);
var energyLeft = batteries.Sum(p => p.Resources.Get(PartResourceLibrary.ElectricityHashcode).amount) / batteries.Sum(p => p.Resources.Get(PartResourceLibrary.ElectricityHashcode).maxAmount);
var openSolars = vessel.mainBody.atmosphere && // true if in atmosphere and there are breakable solarpanels that aren't broken nor retracted
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.isBreakable && p.deployState != ModuleDeployablePart.DeployState.BROKEN &&
p.deployState != ModuleDeployablePart.DeployState.RETRACTED).Count > 0;
if (openSolars && energyLeft > 0.99)
{
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.isBreakable &&
p.deployState == ModuleDeployablePart.DeployState.EXTENDED).ForEach(p => p.Retract());
}
if (energyLeft < 0.05 && Math.Sign(s.wheelThrottle) + Math.Sign(curSpeed) != 0)
{
s.wheelThrottle = 0;
} // save remaining energy by not using it for acceleration
if (openSolars || energyLeft < 0.03)
{
tgtSpeed = 0;
}
if (curSpeed < brakeSpeedLimit && (energyLeft < 0.05 || openSolars))
{
brake = true;
}
if (curSpeed < 0.1 && energyLeft < 0.05 && !waitingForDaylight &&
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.deployState == ModuleDeployablePart.DeployState.EXTENDED).Count > 0)
{
waitingForDaylight = true;
}
}
if (s.wheelThrottle != 0 && (Math.Sign(s.wheelThrottle) + Math.Sign(curSpeed) != 0 || curSpeed < 1))
{
brake = false; // the AP or user want to drive into the direction of momentum so release the brake
}
if (vessel.isActiveVessel)
{
if (GameSettings.BRAKES.GetKeyUp())
{
brake = false; // release the brakes if the user lets go of them
}
if (GameSettings.BRAKES.GetKey())
{
brake = true; // brake if the user brakes and we aren't about to flip
}
}
tractionLimit = (double)Mathf.Clamp((float)tractionLimit, 0, 100);
vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, brake && (StabilityControl && (ControlHeading || ControlSpeed) ? traction >= tractionLimit : true));
// only let go of the brake when losing traction if the AP is driving, otherwise assume the player knows when to let go of it
// also to not constantly turn off the parking brake from going over a small bump
if (brake && curSpeed < 0.1)
{
s.wheelThrottle = 0;
}
}
public override void Drive(FlightCtrlState s) // TODO put the brake in when running out of power to prevent nighttime solar failures on hills, or atleast try to
{ // TODO make distance calculation for 'reached' determination consider the rover and waypoint on sealevel to prevent height differences from messing it up -- should be done now?
if (orbit.referenceBody != lastBody)
{
WaypointIndex = -1; Waypoints.Clear();
}
MechJebWaypoint wp = (WaypointIndex > -1 && WaypointIndex < Waypoints.Count ? Waypoints[WaypointIndex] : null);
var brake = vessel.ActionGroups[KSPActionGroup.Brakes]; // keep brakes locked if they are
curSpeed = Vector3d.Dot(vessel.srf_velocity, vesselState.forward);
CalculateTraction();
speedIntAcc = speedPID.intAccum;
if (wp != null && wp.Body == orbit.referenceBody)
{
if (ControlHeading)
{
heading = Math.Round(HeadingToPos(vessel.CoM, wp.Position), 1);
}
if (ControlSpeed)
{
var nextWP = (WaypointIndex < Waypoints.Count - 1 ? Waypoints[WaypointIndex + 1] : (LoopWaypoints ? Waypoints[0] : null));
var distance = Vector3.Distance(vessel.CoM, wp.Position);
if (wp.Target != null)
{
distance += (float)(wp.Target.srfSpeed * curSpeed) / 2;
}
//var maxSpeed = (wp.MaxSpeed > 0 ? Math.Min((float)speed, wp.MaxSpeed) : speed); // use waypoints maxSpeed if set and smaller than set the speed or just stick with the set speed
var maxSpeed = (wp.MaxSpeed > 0 ? wp.MaxSpeed : speed); // speed used to go towards the waypoint, using the waypoints maxSpeed if set or just stick with the set speed
var minSpeed = (wp.MinSpeed > 0 ? wp.MinSpeed :
(nextWP != null ? TurningSpeed((nextWP.MaxSpeed > 0 ? nextWP.MaxSpeed : speed), heading - HeadingToPos(wp.Position, nextWP.Position)) :
(distance - wp.Radius > 50 ? turnSpeed.val : 1)));
minSpeed = (wp.Quicksave ? 1 : minSpeed);
// ^ speed used to go through the waypoint, using half the set speed or maxSpeed as minSpeed for routing waypoints (all except the last)
var brakeFactor = Math.Max((curSpeed - minSpeed) * 1, 3);
var newSpeed = Math.Min(maxSpeed, Math.Max((distance - wp.Radius) / brakeFactor, minSpeed)); // brake when getting closer
newSpeed = (newSpeed > turnSpeed ? TurningSpeed(newSpeed, headingErr) : newSpeed); // reduce speed when turning a lot
if (LimitAcceleration)
{
newSpeed = curSpeed + Mathf.Clamp((float)(newSpeed - curSpeed), -1.5f, 0.5f);
}
// newSpeed = tgtSpeed + Mathf.Clamp((float)(newSpeed - tgtSpeed), -Time.deltaTime * 8f, Time.deltaTime * 2f);
var radius = Math.Max(wp.Radius, 10);
if (distance < radius)
{
if (WaypointIndex + 1 >= Waypoints.Count) // last waypoint
{
newSpeed = new [] { newSpeed, (distance < radius * 0.8 ? 0 : 1) }.Min();
// ^ limit speed so it'll only go from 1m/s to full stop when braking to prevent accidents on moons
if (LoopWaypoints)
{
WaypointIndex = 0;
}
else
{
newSpeed = 0;
// tgtSpeed.force(newSpeed);
if (curSpeed < brakeSpeedLimit)
{
if (wp.Quicksave)
{
//if (s.mainThrottle > 0) { s.mainThrottle = 0; }
if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
{
WaypointIndex = -1;
ControlHeading = ControlSpeed = false;
QuickSaveLoad.QuickSave();
}
}
else
{
WaypointIndex = -1;
ControlHeading = ControlSpeed = false;
}
}
// else {
// Debug.Log("Is this even getting called?");
// WaypointIndex++;
// }
}
}
else
{
if (wp.Quicksave)
{
//if (s.mainThrottle > 0) { s.mainThrottle = 0; }
newSpeed = 0;
// tgtSpeed.force(newSpeed);
if (curSpeed < brakeSpeedLimit)
{
if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
{
WaypointIndex++;
QuickSaveLoad.QuickSave();
}
}
}
else
{
WaypointIndex++;
}
}
}
brake = brake || ((s.wheelThrottle == 0 || !vessel.isActiveVessel) && curSpeed < brakeSpeedLimit && newSpeed < brakeSpeedLimit);
// ^ brake if needed to prevent rolling, hopefully
tgtSpeed = (newSpeed >= 0 ? newSpeed : 0);
}
}
if (ControlHeading)
{
headingPID.intAccum = Mathf.Clamp((float)headingPID.intAccum, -1, 1);
double instantaneousHeading = vesselState.rotationVesselSurface.eulerAngles.y;
headingErr = MuUtils.ClampDegrees180(instantaneousHeading - heading);
if (s.wheelSteer == s.wheelSteerTrim || FlightGlobals.ActiveVessel != vessel)
{
float spd = Mathf.Min((float)speed, (float)turnSpeed); // if a slower speed than the turnspeed is used also be more careful with the steering
float limit = (Mathf.Abs((float)curSpeed) <= turnSpeed ? 1 : Mathf.Clamp((float)(spd / Mathf.Abs((float)curSpeed)), 0.35f, 1f));
double act = headingPID.Compute(headingErr);
s.wheelSteer = Mathf.Clamp((float)act, -limit, limit);
}
}
// Brake if there is no controler (Pilot eject from seat)
if (BrakeOnEject && vessel.GetReferenceTransformPart() == null)
{
s.wheelThrottle = 0;
// vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
brake = true;
}
else if (ControlSpeed)
{
speedPID.intAccum = Mathf.Clamp((float)speedPID.intAccum, -5, 5);
speedErr = (WaypointIndex == -1 ? speed.val : tgtSpeed) - Vector3d.Dot(vessel.srf_velocity, vesselState.forward);
if (s.wheelThrottle == s.wheelThrottleTrim || FlightGlobals.ActiveVessel != vessel)
{
float act = (float)speedPID.Compute(speedErr);
s.wheelThrottle = !LimitAcceleration?Mathf.Clamp((float)act, -1, 1) : // I think I'm using these ( ? : ) a bit too much
(traction == 0 ? 0 : (act < 0 ? Mathf.Clamp(act, -1f, 1f) : (lastThrottle + Mathf.Clamp(act - lastThrottle, -0.005f, 0.005f)) * (traction < tractionLimit ? -1 : 1)));
if (curSpeed < 0 & s.wheelThrottle < 0)
{
s.wheelThrottle = 0;
} // don't go backwards
// if (Mathf.Sign(act) + Mathf.Sign(s.wheelThrottle) == 0) { s.wheelThrottle = Mathf.Clamp(act, -1f, 1f); }
if (speedErr < -1 && StabilityControl && Mathf.Sign(s.wheelThrottle) + Mathf.Sign((float)curSpeed) == 0) // StabilityControl && traction > 50 &&
// vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
{
brake = true;
}
// else if (!stabilityControl || traction <= 50 || speedErr > -0.2 || Mathf.Sign(s.wheelThrottle) + Mathf.Sign((float)curSpeed) != 0) {
// vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, (GameSettings.BRAKES.GetKey() && vessel.isActiveVessel));
// }
lastThrottle = s.wheelThrottle;
}
}
if (StabilityControl)
{
if (!core.attitude.users.Contains(this))
{
core.attitude.users.Add(this);
// line.enabled = true;
}
// float scale = Vector3.Distance(FlightCamera.fetch.mainCamera.transform.position, vessel.CoM) / 900f;
// line.SetPosition(0, vessel.CoM);
// line.SetPosition(1, vessel.CoM + hit.normal * 5);
// line.SetWidth(0, scale + 0.1f);
var fSpeed = (float)curSpeed;
// if (Mathf.Abs(fSpeed) >= turnSpeed * 0.75) {
Vector3 fwd = (Vector3)(traction > 0 ? // V when the speed is low go for the vessels forward, else with a bit of velocity
// ((Mathf.Abs(fSpeed) <= turnSpeed ? vesselState.forward : vessel.srf_velocity / 4) - vessel.transform.right * s.wheelSteer) * Mathf.Sign(fSpeed) :
// // ^ and then add the steering
vesselState.forward * 4 - vessel.transform.right * s.wheelSteer * Mathf.Sign(fSpeed) : // and then add the steering
vessel.srf_velocity); // in the air so follow velocity
Vector3.OrthoNormalize(ref norm, ref fwd);
var quat = Quaternion.LookRotation(fwd, norm);
// if (traction > 0 || speed <= turnSpeed) {
// var u = new Vector3(0, 1, 0);
//
// var q = FlightGlobals.ship_rotation;
// var q_s = quat;
//
// var q_u = new Quaternion(u.x, u.y, u.z, 0);
// var a = Quaternion.Dot(q, q_s * q_u);
// var q_qs = Quaternion.Dot(q, q_s);
// var b = (a == 0) ? Math.Sign(q_qs) : (q_qs / a);
// var g = b / Mathf.Sqrt((b * b) + 1);
// var gu = Mathf.Sqrt(1 - (g * g)) * u;
// var q_d = new Quaternion() { w = g, x = gu.x, y = gu.y, z = gu.z };
// var n = q_s * q_d;
//
// quat = n;
// }
core.attitude.attitudeTo(quat, AttitudeReference.INERTIAL, this);
// }
}
if (BrakeOnEnergyDepletion)
{
var batteries = vessel.Parts.FindAll(p => p.Resources.Contains("ElectricCharge") && p.Resources["ElectricCharge"].flowState);
var energyLeft = batteries.Sum(p => p.Resources["ElectricCharge"].amount) / batteries.Sum(p => p.Resources["ElectricCharge"].maxAmount);
var openSolars = vessel.mainBody.atmosphere && // true if in atmosphere and there are breakable solarpanels that aren't broken nor retracted
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.isBreakable && p.panelState != ModuleDeployableSolarPanel.panelStates.BROKEN &&
p.panelState != ModuleDeployableSolarPanel.panelStates.RETRACTED).Count > 0;
if (openSolars && energyLeft > 0.99)
{
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.isBreakable &&
p.panelState == ModuleDeployableSolarPanel.panelStates.EXTENDED).ForEach(p => p.Retract());
}
if (energyLeft < 0.05 && Mathf.Sign(s.wheelThrottle) + Mathf.Sign((float)curSpeed) != 0)
{
s.wheelThrottle = 0;
} // save remaining energy by not using it for acceleration
if (openSolars || energyLeft < 0.03)
{
tgtSpeed = 0;
}
if (curSpeed < brakeSpeedLimit && (energyLeft < 0.05 || openSolars))
{
brake = true;
}
if (curSpeed < 0.1 && energyLeft < 0.05 && !waitingForDaylight &&
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.panelState == ModuleDeployableSolarPanel.panelStates.EXTENDED).Count > 0)
{
waitingForDaylight = true;
}
}
// brake = brake && (s.wheelThrottle == 0); // release brake if the user or AP want to drive
if (s.wheelThrottle != 0 && Mathf.Sign(s.wheelThrottle) + Mathf.Sign((float)curSpeed) != 0)
{
brake = false; // the AP or user want to drive into the direction of momentum so release the brake
}
if (vessel.isActiveVessel)
{
if (GameSettings.BRAKES.GetKeyUp())
{
brake = false; // release the brakes if the user lets go of them
}
if (GameSettings.BRAKES.GetKey())
{
brake = true; // brake if the user brakes and we aren't about to flip
}
}
tractionLimit = (double)Mathf.Clamp((float)tractionLimit, 0, 100);
vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, brake && (StabilityControl && curSpeed > brakeSpeedLimit ? traction >= tractionLimit : true));
// ^ brake but hopefully prevent flipping over, assuming the user set up the limit right
if (brake && curSpeed < 0.1)
{
s.wheelThrottle = 0;
}
}
public override void Drive(FlightCtrlState s)
{
UpdatePID();
//SpeedHold (set AccelerationTarget automatically to hold speed)
if (SpeedHoldEnabled)
{
double spd = vesselState.speedSurface;
cur_acc = (spd - _spd) / Time.fixedDeltaTime;
_spd = spd;
RealAccelerationTarget = (SpeedTarget - spd) / 4;
a_err = (RealAccelerationTarget - cur_acc);
AccelerationPIDController.intAccum = MuUtils.Clamp(AccelerationPIDController.intAccum, -1 / AccKi, 1 / AccKi);
double t_act = AccelerationPIDController.Compute(a_err);
if (!double.IsNaN(t_act))
{
core.thrust.targetThrottle = (float)MuUtils.Clamp(t_act, 0, 1);
}
else
{
core.thrust.targetThrottle = 0.0f;
AccelerationPIDController.Reset();
}
}
//AltitudeHold (set VertSpeed automatically to hold altitude)
if (AltitudeHoldEnabled)
{
RealVertSpeedTarget = convertAltitudeToVerticalSpeed(AltitudeTarget - vesselState.altitudeASL);
RealVertSpeedTarget = UtilMath.Clamp(RealVertSpeedTarget, -VertSpeedTarget, VertSpeedTarget);
}
else
{
RealVertSpeedTarget = VertSpeedTarget;
}
pitch_err = roll_err = yaw_err = 0;
pitch_act = roll_act = yaw_act = 0;
//VertSpeedHold
if (VertSpeedHoldEnabled)
{
// NOTE: 60-to-1 rule:
// deltaAltitude = 2 * PI * r * deltaPitch / 360
// Vvertical = 2 * PI * TAS * deltaPitch / 360
// deltaPitch = Vvertical / Vhorizontal * 180 / PI
double deltaVertSpeed = RealVertSpeedTarget - vesselState.speedVertical;
double adjustment = 180 / vesselState.speedSurface * deltaVertSpeed / Math.PI;
RealPitchTarget = vesselState.vesselPitch + adjustment;
RealPitchTarget = UtilMath.Clamp(RealPitchTarget, -PitchDownLimit, PitchUpLimit);
pitch_err = MuUtils.ClampDegrees180(RealPitchTarget - vesselState.vesselPitch);
PitchPIDController.intAccum = UtilMath.Clamp(PitchPIDController.intAccum, -100 / PitKi, 100 / PitKi);
pitch_act = PitchPIDController.Compute(pitch_err) / 100;
//Debug.Log (p_act);
if (double.IsNaN(pitch_act))
{
PitchPIDController.Reset();
}
else
{
s.pitch = Mathf.Clamp((float)pitch_act, -1, 1);
}
}
//HeadingHold
double curFlightPath = vesselState.HeadingFromDirection(vesselState.forward);
// NOTE: we can not use vesselState.vesselHeading here because it interpolates headings internally
// i.e. turning from 1° to 359° will end up as (1+359)/2 = 180°
// see class MovingAverage for more details
curr_yaw = vesselState.currentHeading - curFlightPath;
if (HeadingHoldEnabled)
{
double toturn = MuUtils.ClampDegrees180(HeadingTarget - curFlightPath);
if (Math.Abs(toturn) < 0.2)
{
// yaw for small adjustments
RealYawTarget = MuUtils.Clamp(toturn * 2, -YawLimit, YawLimit);
RealRollTarget = 0;
}
else
{
// roll for large adjustments
RealYawTarget = 0;
RealRollTarget = MuUtils.Clamp(toturn * 2, -RollLimit, RollLimit);
}
}
else
{
RealRollTarget = RollTarget;
RealYawTarget = 0;
}
if (RollHoldEnabled)
{
RealRollTarget = UtilMath.Clamp(RealRollTarget, -BankAngle, BankAngle);
RealRollTarget = UtilMath.Clamp(RealRollTarget, -RollLimit, RollLimit);
roll_err = MuUtils.ClampDegrees180(RealRollTarget - -vesselState.currentRoll);
RollPIDController.intAccum = MuUtils.Clamp(RollPIDController.intAccum, -100 / RolKi, 100 / RolKi);
roll_act = RollPIDController.Compute(roll_err) / 100;
if (double.IsNaN(roll_act))
{
RollPIDController.Reset();
}
else
{
s.roll = Mathf.Clamp((float)roll_act, -1, 1);
}
}
if (HeadingHoldEnabled)
{
RealYawTarget = UtilMath.Clamp(RealYawTarget, -YawLimit, YawLimit);
yaw_err = MuUtils.ClampDegrees180(RealYawTarget - curr_yaw);
YawPIDController.intAccum = MuUtils.Clamp(YawPIDController.intAccum, -100 / YawKi, 100 / YawKi);
yaw_act = YawPIDController.Compute(yaw_err) / 100;
if (double.IsNaN(yaw_act))
{
YawPIDController.Reset();
}
else
{
s.yaw = Mathf.Clamp((float)yaw_act, -1, 1);
}
}
}