private double GetBrakingDistanceThreshold(double shipAcceleration, Vector3D shipVelocityVec)
{
double forceSum = GetBrakingForce();
//some arbitrary number that will stop NaN cases
if (forceSum == 0)
{
return(1000d);
}
double mass = referenceBlock.CalculateShipMass().PhysicalMass;
//Echo($"Mass: {mass.ToString()}");
double maxDeceleration = forceSum / mass;
double effectiveDeceleration = maxDeceleration - shipAcceleration;
double wantedDeceleration = effectiveDeceleration * BURN_THRUST_PERCENTAGE;
//Echo($"Decel: {deceleration.ToString()}");
// If speed mod
double maxSpeed = MAX_SPEED;
if (shipVelocityVec.LengthSquared() > MAX_SPEED * MAX_SPEED)
{
maxSpeed = shipVelocityVec.Length();
}
//cushion to account for discrete time errors
double safetyCushion = maxSpeed * timeMaxCycle * SafetyCushionConstant;
//derived from: vf^2 = vi^2 + 2*a*d
//added for safety :)
double distanceToStop = shipVelocityVec.LengthSquared() / (2 * wantedDeceleration) + safetyCushion;
return(distanceToStop);
}
double GetBrakingAltitudeThreshold()
{
double forceSum = 0;
foreach (IMyThrust thisThrust in brakingThrusters)
{
forceSum += thisThrust.MaxEffectiveThrust;
}
if (forceSum == 0)
{
return(1000d); //some arbitrary number that will stop NaN cases
}
//Echo($"Force: {forceSum.ToString()}");
//Echo($"Speed: {downSpeed.ToString()}");
double mass = referenceBlock.CalculateShipMass().PhysicalMass;
//Echo($"Mass: {mass.ToString()}");
double deceleration = (forceSum / mass - gravityVecMagnitude) * burnThrustPercentage; //Echo($"Decel: {deceleration.ToString()}");
double safetyCushion = maxSpeed * timeMaxCycle * SafetyCushionConstant; //cushion to account for discrete time errors
//derived from: vf^2 = vi^2 + 2*a*d
double distanceToStop = shipVelocityVec.LengthSquared() / (2 * deceleration) + safetyCushion + altitudeSafetyCushion; //added for safety :)
return(distanceToStop);
}
double GetShipMass()
{
double totalMass = rc.CalculateShipMass().TotalMass; // ship total mass including cargo mass
double baseMass = rc.CalculateShipMass().BaseMass; // mass of the ship without cargo
double cargoMass = totalMass - baseMass; // mass of the cargo
return(baseMass + (cargoMass / inventoryMultiplier)); // the mass the game uses for physics calculation
}
private double calculateGravityForce()
{
double gravityVectorLength = myShipController.GetNaturalGravity().Length();
float currentMass = myShipController.CalculateShipMass().TotalMass;
return(gravityVectorLength * currentMass);
}
private void UpdateMass()
{
var mass = shipController.CalculateShipMass();
baseMass = mass.BaseMass;
totalMass = mass.TotalMass;
}
/// <summary>
/// returns the optimal max speed based on available braking thrust and ship mass
/// </summary>
/// <param name="thrustList">Thrusters to use</param>
/// <param name="distance">current distance to target location</param>
/// <returns>optimal max speed (may be game max speed)</returns>
public double CalculateOptimalSpeed(List <IMyTerminalBlock> thrustList, double distance)
{
//
// Echo("#thrusters=" + thrustList.Count.ToString());
if (thrustList.Count < 1)
{
return(thisProgram.wicoControl.fMaxWorldMps);
}
MyShipMass myMass;
myMass = tmShipController.CalculateShipMass();
double maxThrust = thisProgram.wicoThrusters.calculateMaxThrust(thrustList);
double maxDeltaV = maxThrust / myMass.PhysicalMass;
// Magic..
double optimalV, secondstozero, stoppingM;
optimalV = ((distance * .75) / 2) / (maxDeltaV); // determined by experimentation and black magic
// Echo("COS");
do
{
// Echo("COS:DO");
secondstozero = optimalV / maxDeltaV;
stoppingM = optimalV / 2 * secondstozero;
if (stoppingM > distance)
{
optimalV *= 0.85;
}
// Echo("stoppingM=" + stoppingM.ToString("F1") + " distance=" + distance.ToString("N1"));
}while (stoppingM > distance);
// Echo("COS:X");
return(optimalV);
}
public void Update(Vector3D target)
{
MatrixD transpose = MatrixD.Transpose(rc.WorldMatrix);
Vector3D meToTarget = rc.WorldMatrix.Translation - target;
Vector3D localError = Vector3D.TransformNormal(meToTarget, transpose);
localError.Y = 0;
if (localError.X > -0.5 && localError.X < 0.5)
{
localError.X = 0;
}
if (localError.Z > -0.5 && localError.Z < 0.5)
{
localError.Z = 0;
}
float correction = (float)forwardPID.Control(localError.Z);
float force = correction * rc.CalculateShipMass().TotalMass;
float rightLeft = (float)anglePID.Control(-localError.X);
Vector3D localVelocity = Vector3D.TransformNormal(rc.GetShipVelocities().LinearVelocity, transpose);
float angle = -rightLeft;
if (localVelocity.Z < 0)
{
angle *= -1;
}
foreach (Wheel w in wheels)
{
IMyMotorSuspension wheel = w.block;
if (first)
{
Vector3D center = Vector3D.TransformNormal(rc.CenterOfMass - rc.GetPosition(), transpose);
Vector3D local = Vector3D.TransformNormal(wheel.GetPosition() - rc.GetPosition(), transpose);
wheel.InvertSteer = (local.Z > center.Z);
wheel.InvertPropulsion = (wheel.Orientation.Left != rc.Orientation.Forward);
wheel.Brake = false;
}
if (wheel.Steering)
{
wheel.SetValueFloat("Steer override", angle);
}
if (wheel.Propulsion)
{
float maxForce = w.maxForce;
if (maxForce <= 0)
{
continue;
}
float percent = MathHelper.Clamp(force / maxForce, -1, 1);
force -= percent * maxForce;
wheel.SetValueFloat("Propulsion override", percent);
}
}
first = false;
}
public Program()
{
// The constructor, called only once every session and
// always before any other method is called. Use it to
// initialize your script.
//
// The constructor is optional and can be removed if not
// needed.
//
// It's recommended to set Runtime.UpdateFrequency
// here, which will allow your script to run itself without a
// timer block.
Runtime.UpdateFrequency = UpdateFrequency.Update1;
g = GridTerminalSystem.GetBlockWithName("Gyro") as IMyGyro;
sc = GridTerminalSystem.GetBlockWithName("Cockpit") as IMyShipController;
lcd = GridTerminalSystem.GetBlockWithName("LCD") as IMyTextPanel;
thrusters = new List <IMyThrust>();
GridTerminalSystem.GetBlockGroupWithName("Thrusters").GetBlocksOfType <IMyThrust>(thrusters);
hDamp = false;
manualAlt = true;
vDamp = false;
targAltitude = GetAltitude();
mass = sc.CalculateShipMass().PhysicalMass;
curSpeed = 0;
lastHeading = sc.WorldMatrix.Forward;
}
public void ApplyAccel(Vector3D accel)
{
if (accel.Equals(Vector3D.Zero, 0.1))
{
accel = Vector3D.Zero;
}
Vector3D thrust = accel * rc.CalculateShipMass().TotalMass;
foreach (IMyThrust t in thrusters)
{
if (!t.IsFunctional)
{
continue;
}
float outputThrust = (float)Vector3D.Dot(t.WorldMatrix.Forward, thrust);
if (outputThrust > 0)
{
float outputProportion = MathHelper.Clamp(outputThrust / t.MaxEffectiveThrust, minThrust / t.MaxThrust, 1);
t.ThrustOverridePercentage = outputProportion;
thrust -= t.WorldMatrix.Forward * outputProportion * t.MaxEffectiveThrust;
}
else
{
t.ThrustOverride = minThrust;
}
}
}
double CalculateRequiredThrust()
{
var mass = controlBlock.CalculateShipMass().TotalMass;
var requiredThrust = mass * gravityStrength;
return(requiredThrust);
}
private bool Travel()
{
double distToTarget = GetDistanceToTarget();
if (distToTarget < 2 * shipCenterToEdge)
{
return(true);
}
bool isEnd = false;
bool shouldBrake = CheckBrake();
if (shouldBrake)
{
ManageBrake();
if (timeSpentStationary > SHUTDOWN_TIME)
{
Debug("timeSpentStationary", timeSpentStationary);
isEnd = true;
}
}
else
{
float shipMass = referenceBlock.CalculateShipMass().PhysicalMass;
float speed = (float)referenceBlock.GetShipSpeed();
float secondsUntilVmax = 1;
float minLimitToZero = 1 / 100;
float maxLimitPercent = 98 / 100;
// F = m*a
// v_t = a_0*t + v_0 => a_0 = (v_t - v_0) / t
// F = m * (v_t - v_0) / t
float maxForceRequiered = shipMass * (maxInitialSpeed - speed) / secondsUntilVmax;
float percentForceRequiered = maxForceRequiered / GetAcceleratingForce();
if (percentForceRequiered > maxLimitPercent)
{
percentForceRequiered = maxLimitPercent;
}
if (percentForceRequiered < minLimitToZero)
{
percentForceRequiered = 0F;
}
GetThrusters(ThrustersFlag.Accelerating).ForEach(t => t.ThrustOverridePercentage = percentForceRequiered);
}
return(isEnd);
}
public static int GetThrustCapacityRemaining(List <IMyThrust> blocks, IMyShipController shipController, Action <string> Echo)
{
//TODO: Need to calculate this
float localGravity = 9.81f;
int newtonsRequiredToLiftShip = (int)(shipController.CalculateShipMass().TotalMass *localGravity);
float maximumThrustInNewtons = BlockUtils.GetSumOfBlockAttribute <IMyThrust>(b => b.MaxEffectiveThrust, blocks);
int percent = (int)(newtonsRequiredToLiftShip / maximumThrustInNewtons * 100);
return(percent);
}
public void CheckForMassChange()
{
var Masses = cockpit.CalculateShipMass();
shipMass = Masses.PhysicalMass; //In kg
if (previousShipMass != shipMass)
{
GatherBasicData();
}
previousShipMass = shipMass;
}
public override string Get(Dictionary <string, int> storage)
{
IMyShipController c = owner.gts.GetBlockWithName(COCKPIT_NAME) as IMyShipController;
int m = (int)(c.CalculateShipMass().TotalMass / 1000.0);
string s = m.ToString();
if (vPadWidth <= 0)
{
return(s);
}
return(PadLeft(s, vPadWidth));
}
private void UdjustSuspensionStrength()
{
float gravityFactor = (float)_controller.GetNaturalGravity().Length() / 9.81f;
float truckCurrentMass = _controller.CalculateShipMass().PhysicalMass *gravityFactor;
float massToWheelsDifference = truckCurrentMass / _suspensions.Count;
float _truckSuspensionStrength = (float)Math.Sqrt(massToWheelsDifference / _suspensionSoftnessFactor);
SuspensionStrengthChangedEventArgs args = new SuspensionStrengthChangedEventArgs
{
Strength = _truckSuspensionStrength
};
ChangeTruckSuspensionStrength?.Invoke(this, args);
}
//disconnects the car from any connector, releases the hand brake, takes the batteries off recharge and sets the dampeners
void disconnect()
{
float currentMass = controller.CalculateShipMass().PhysicalMass;
float massRatio = currentMass / maxMass;
//we only want to allow the connectors to unlock if the max mass is not surpassed
if (massRatio <= 1)
{
for (int i = 0; i < wheels.Count; i++)
{
wheels[i].setStrenght(massRatio);
}
}
}
public bool Update()
{
if (Controller != null)
{
//Masses of ship
val = Controller.CalculateShipMass().PhysicalMass;
if (val != Value)
{
Value = val;
return(true);
}
}
return(false);
}
void UpdateTelemetry()
{
prevSpeed = currSpeed;
currAccl = (currSpeed = sc.GetShipSpeed()) - prevSpeed;
gravity = (gravityStrength = sc.GetNaturalGravity().Length()) / 9.81;
if (shipMassUpdateTick < Pgm.totalTicks)
{
shipMassUpdateTick = Pgm.totalTicks + TPS;
shipMass = sc.CalculateShipMass();
}
shipWeight = gravityStrength * shipMass.PhysicalMass; // or -> shipMass.TotalMass
prevAltitude = altitudeSurface;
if (sc.TryGetPlanetElevation(MyPlanetElevation.Surface, out altitudeSurface))
{
altitudeDiff = altitudeSurface - prevAltitude;
}
else
{
altitudeSurface = altitudeDiff = double.NaN;
}
atmosphereDensity = parachute?.Atmosphere ?? float.NaN;
if (null != downCamera)
{
if (downCamera.CanScan(1000))
{
MyDetectedEntityInfo dei = downCamera.Raycast(1000);
double len = 0;
if (null != dei.HitPosition)
{
Vector3D hp = (Vector3D)dei.HitPosition;
len = (hp - downCamera.CubeGrid.GetPosition()).Length();
}
raycastName = dei.Name + "/" + len; //downCamera.CubeGrid.GetPosition(); //dei.HitPosition.ToString();
}
else
{
raycastName = downCamera.AvailableScanRange.ToString();
}
}
}
public bool Update()
{
if (Controller != null)
{
//Masses of ship
masses = Controller.CalculateShipMass();
val = masses.PhysicalMass - masses.BaseMass;
if (masses.PhysicalMass == 0)
{
val = 0;
}
if (val != Value)
{
Value = val;
return(true);
}
}
return(false);
}
public float CalculateThrustToHover()
{
var gravityDir = controller.GetNaturalGravity();
var weight = controller.CalculateShipMass().TotalMass *gravityDir.Length();
var velocity = controller.GetShipVelocities().LinearVelocity;
gravityDir.Normalize();
var gravityMatrix = Matrix.Invert(Matrix.CreateFromDir(gravityDir));
velocity = Vector3D.Transform(velocity, gravityMatrix);
if (Vector3.Transform(controller.WorldMatrix.GetOrientation().Down, gravityMatrix).Z < 0)
{
return((float)(weight + weight * -velocity.Z));
}
else
{
return(-(float)(weight + weight * -velocity.Z));
}
}
private void CalculateLiftThrustUsage(IMyShipController controller, List <IMyThrust> thrusters)
{
float mass = controller.CalculateShipMass().PhysicalMass;
float gravityStrength = (float)(controller.GetNaturalGravity().Length() / 9.81);
LiftThrustNeeded = (mass * (float)gravityStrength / 100) * 1000;
Vector3 gravity = controller.GetNaturalGravity();
LiftThrustAvailable = 0;
thrusters.ForEach(thruster =>
{
if (thruster.IsWorking)
{
Vector3D thrusterDirection = thruster.WorldMatrix.Forward;
double upDot = Vector3D.Dot(thrusterDirection, Vector3.Normalize(gravity));
LiftThrustAvailable += (thruster.MaxEffectiveThrust * (float)upDot);
}
});
}
private void CalculateStopDistance(IMyShipController controller, List <IMyThrust> thrusters)
{
float mass = controller.CalculateShipMass().PhysicalMass;
double stopThrustAvailable = 0;
int disabledThrusters = 0;
thrusters.ForEach(thruster =>
{
if (!thruster.IsWorking)
{
disabledThrusters++;
}
if (thruster.IsFunctional)
{
stopThrustAvailable += thruster.MaxEffectiveThrust;
}
});
StopThrustersWarning = disabledThrusters > 0;
double deacceleration = -stopThrustAvailable / mass;
double currentSpeed = controller.GetShipSpeed();
StoppingTime = (float)(-currentSpeed / deacceleration);
StoppingDistance = (float)(currentSpeed * StoppingTime + (deacceleration * StoppingTime * StoppingTime) / 2.0f);
}
public bool CalculateHoverThrust(IMyShipController ShipController, List <IMyTerminalBlock> thrusters, out float atmoPercent, out float hydroPercent, out float ionPercent)
{
atmoPercent = 0;
hydroPercent = 0;
ionPercent = 0;
double ionThrust = calculateMaxThrust(thrusters, thrustion);
double atmoThrust = calculateMaxThrust(thrusters, thrustatmo);
double hydroThrust = calculateMaxThrust(thrusters, thrusthydro);
MyShipMass myMass;
myMass = ShipController.CalculateShipMass();
Vector3D vGrav = ShipController.GetNaturalGravity();
double dGravity = vGrav.Length() / 9.81;
double hoverthrust = myMass.PhysicalMass * dGravity * 9.810;
if (atmoThrust > 0)
{
if (atmoThrust < hoverthrust)
{
atmoPercent = 100;
hoverthrust -= atmoThrust;
}
else
{
atmoPercent = (float)(hoverthrust / atmoThrust * 100);
if (atmoPercent > 0)
{
hoverthrust -= (atmoThrust * atmoPercent / 100);
}
}
}
// Echo("ALeft over thrust=" + hoverthrust.ToString("N0"));
if (ionThrust > 0 && hoverthrust > 0)
{
if (ionThrust < hoverthrust)
{
ionPercent = 100;
hoverthrust -= ionThrust;
}
else
{
ionPercent = (float)(hoverthrust / ionThrust * 100);
if (ionPercent > 0)
{
hoverthrust -= ((ionThrust * ionPercent) / 100);
}
}
}
// Echo("ILeft over thrust=" + hoverthrust.ToString("N0"));
if (hydroThrust > 0 && hoverthrust > 0)
{
if (hydroThrust < hoverthrust)
{
hydroPercent = 100;
hoverthrust -= hydroThrust;
}
else
{
hydroPercent = (float)(hoverthrust / hydroThrust * 100);
if (hydroPercent > 0)
{
hoverthrust -= ((hydroThrust * hydroPercent) / 100);
}
;
}
}
// Echo("Atmo=" + ((atmoThrust * atmoPercent) / 100).ToString("N0"));
// Echo("ion=" + ((ionThrust * ionPercent) / 100).ToString("N0"));
// Echo("hydro=" + ((hydroThrust * hydroPercent) / 100).ToString("N0"));
// Echo("Left over thrust=" + hoverthrust.ToString("N0"));
if (hoverthrust > 0)
{
return(false);
}
return(true);
}
public void ProcessCalculations(IMyShipController pController)
{
mDirectionStatsList = new List <CGI_ThrusterDirectionStats>();
double aPhysicalMass = pController.CalculateShipMass().PhysicalMass;
Dictionary <Base6Directions.Direction, double> aLocalGravity = new Dictionary <Base6Directions.Direction, double>();
if (COMPENSATE_GRAVITY)
{
/// this is the local gravity acceleration
Vector3D aGravity = pController.GetNaturalGravity();
Vector3D aUp = pController.WorldMatrix.Up;
Vector3D aLeft = pController.WorldMatrix.Left;
Vector3D aForward = pController.WorldMatrix.Forward;
aLocalGravity[Base6Directions.Direction.Forward] = aGravity.Dot(aForward); // FORWARD;
aLocalGravity[Base6Directions.Direction.Backward] = -aLocalGravity[Base6Directions.Direction.Forward];
aLocalGravity[Base6Directions.Direction.Up] = aGravity.Dot(aUp); // UP;
aLocalGravity[Base6Directions.Direction.Down] = -aLocalGravity[Base6Directions.Direction.Up];
aLocalGravity[Base6Directions.Direction.Left] = aGravity.Dot(aLeft); // UP;
aLocalGravity[Base6Directions.Direction.Right] = -aLocalGravity[Base6Directions.Direction.Left];
}
foreach (KeyValuePair <Base6Directions.Direction, List <IMyThrust> > aPair in myThrustDirections)
{
CGI_ThrusterDirectionStats aDirectionStats = new CGI_ThrusterDirectionStats();
Base6Directions.Direction aKey = aPair.Key;
List <IMyThrust> aValue = aPair.Value;
aDirectionStats.mDirection = aKey;
foreach (IMyThrust aThruster in aValue)
{
if (aThruster.Enabled && aThruster.IsFunctional)
{
aDirectionStats.mDirectionForceCurrent += aThruster.CurrentThrust;
aDirectionStats.mDirectionForceEffective += aThruster.MaxEffectiveThrust;
aDirectionStats.mDirectionForceMax += aThruster.MaxThrust;
aDirectionStats.mThrusters += 1;
}
}
if (COMPENSATE_GRAVITY)
{
double aGravityDirectionForce = aLocalGravity[aKey] * aPhysicalMass;
aDirectionStats.mDirectionForceCurrent -= aGravityDirectionForce;
aDirectionStats.mDirectionForceEffective -= aGravityDirectionForce;
aDirectionStats.mDirectionForceMax -= aGravityDirectionForce;
}
aDirectionStats.mAccelerationMax = aDirectionStats.mDirectionForceMax / aPhysicalMass;
aDirectionStats.mAccelerationEffective = aDirectionStats.mDirectionForceEffective / aPhysicalMass;
aDirectionStats.mAccelerationCurrent = aDirectionStats.mDirectionForceCurrent / aPhysicalMass;
aDirectionStats.mEfficiency = aDirectionStats.mDirectionForceCurrent / aDirectionStats.mDirectionForceMax;
mDirectionStatsList.Add(aDirectionStats);
}
ProcessVelocities(pController);
}
/// <summary>
/// Gathers the blocks that relate to the ship. This includes:<br />The cockpit, thrusters, gyros, (main)
/// connector.
/// </summary>
public void GatherBasicData()
{
if (!firstTime && !parent_program.scriptEnabled)
{
parent_program.shipIOHandler.Echo(
"RE-INITIALIZED\nSome change was detected\nso I have re-checked ship data, " +
parent_program.your_title + ".");
}
cockpit = FindCockpit();
if (cockpit != null)
{
var Masses = cockpit.CalculateShipMass();
shipMass = Masses.PhysicalMass; //In kg
previousShipMass = shipMass;
}
else
{
parent_program.shipIOHandler.Error(
"The ship systems analyzer couldn't find some sort of cockpit or remote control.\nPlease check you have one of these, " +
parent_program.your_title + ".");
}
if (parent_program.Me.CubeGrid.GridSize == 0.5)
{
isLargeShip = false;
}
else
{
isLargeShip = true;
}
//myConnector = FindConnector();
thrusters = FindThrusters();
gyros = FindGyros();
var antenna_result = "";
if (parent_program.enable_antenna_function)
{
antenna_result = parent_program.antennaHandler.CheckAntenna();
}
if (!parent_program.errorState)
{
if (firstTime)
{
parent_program.shipIOHandler.Echo("Waiting for orders, " + parent_program.your_title + ".\n");
if (!parent_program.extra_info)
{
parent_program.shipIOHandler.Echo("Ready for Waypoints.\n");
}
if (antenna_result != "")
{
parent_program.shipIOHandler.Echo(antenna_result);
}
if (parent_program.extra_info)
{
if (shipMass != 0)
{
parent_program.shipIOHandler.Echo("Mass: " + shipMass);
}
parent_program.shipIOHandler.Echo("Thruster count: " + thrusters.Count);
parent_program.shipIOHandler.Echo("Gyro count: " + gyros.Count);
parent_program.shipIOHandler.Echo("Main control: " + cockpit.CustomName);
parent_program.shipIOHandler.Echo("Is large ship: " + isLargeShip);
//if (parent_program.homeLocations.Count > 0) {
parent_program.shipIOHandler.OutputHomeLocations();
//}
}
}
}
Populate6ThrusterGroups();
parent_program.shipIOHandler.EchoFinish();
firstTime = false;
}
void GetMovementVectors(Vector3D target, IMyShipController controller, IMyTerminalBlock reference, float maxThrust, float maxSpeed, out Vector3D AutopilotMoveIndicator, ref Vector3D D, ref Vector3D I)
{
if (controller == null)
{
AutopilotMoveIndicator = Vector3D.Zero;
return;
}
var speed = (float)(controller.GetShipVelocities().LinearVelocity - targetDrift).Length();
Vector3D currentVelocity = controller.GetShipVelocities().LinearVelocity;
float aMax = 0.8f * maxThrust / controller.CalculateShipMass().PhysicalMass;
Vector3D desiredVelocity = targetDrift;
Vector3D posError = target - reference.WorldMatrix.Translation;
var distance = (float)posError.Length();
posError.Normalize();
if (target != Vector3D.Zero)
{
float desiredSpeed = Math.Min((float)Math.Sqrt(2f * aMax * distance) * 0.01f * (100 - (float)targetDrift.Length()), maxSpeed);
desiredSpeed = Math.Min(distance * distance * 2f, desiredSpeed);
desiredVelocity += posError * desiredSpeed;
}
Vector3D adjustVector = currentVelocity - targetDrift - VectorHelpers.VectorProjection(currentVelocity - targetDrift, desiredVelocity - targetDrift);
if (adjustVector.Length() < (currentVelocity - targetDrift).Length() * 0.1)
{
adjustVector = Vector3.Zero;
}
Vector3D Error = (desiredVelocity - currentVelocity - adjustVector * 1) * 60 / (aMax * kRunEveryXUpdates);
float kP = TP;
float kI = TI;
float kD = TD;
if (Error.LengthSquared() > 1)
{
Error.Normalize();
}
else
{
kP = TP2;
kI = TI2;
kD = TD2;
}
if (D == Vector3.Zero)
{
D = Error;
}
AutopilotMoveIndicator = kP * Error + kD * (Error - D) * kInverseTimeStep + kI * I;
// decrease speed when close and slow
if (distance < 10 && speed < 10)
{
AutopilotMoveIndicator *= 0.5f;
}
if (AutopilotMoveIndicator.Length() > 1)
{
AutopilotMoveIndicator /= AutopilotMoveIndicator.Length();
}
I += Error * kRunEveryXUpdates;
if (I.Length() > 5)
{
I *= 5 / I.Length();
}
D = Error;
// if close enough, stop.
if (targetDrift == Vector3D.Zero && distance < 0.25f && speed < 0.25f)
{
targetPosition = Vector3.Zero;
AutopilotMoveIndicator = Vector3.Zero;
I = Vector3.Zero;
D = Vector3.Zero;
}
}
public void Main(string input)
{
lcds = SearchBlocksWithName <IMyTextPanel>(lcdSearchName);
ClearOutput();
if (input == "start")
{
Runtime.UpdateFrequency = UpdateFrequency.Update10;
Autopilot = true;
}
IMyShipController controlBlock = (IMyShipController)GridTerminalSystem.GetBlockWithName(ShipControllerName);
double altitude = 0;
bool InsideNaturalGravity = controlBlock.TryGetPlanetElevation(MyPlanetElevation.Surface, out altitude);
Vector3D velocity3D = controlBlock.GetShipVelocities().LinearVelocity;
if (!InsideNaturalGravity)
{
if (Autopilot)
{
WriteLine("Waiting for entering natural gravity");
if (input == "stop")
{
Autopilot = false;
WriteLine("Autopilot deactivated (manually)");
}
}
return;
}
else
{
if (Autopilot && AutoFall)
{
if (!AutoFallUsed)
{
input = "fall";
AutoFallUsed = true;
}
}
}
List <IMyThrust> thrusters = GetBlocksInGroup <IMyThrust>(HydrogenThrustersGroupName);
ThrustController thrustController = new ThrustController(thrusters);
gyros = GetBlocksOfType <IMyGyro>();
gyroController = new GyroController(controlBlock, gyros, Base6Directions.Direction.Down, RotationSpeedLimit);
Vector3D gravity = controlBlock.GetNaturalGravity();
Vector3D position = controlBlock.GetPosition(); // ship coords
double gravityStrength = gravity.Length(); // gravity in m/s^2
double totalMass = controlBlock.CalculateShipMass().TotalMass; // ship total mass including cargo mass
double baseMass = controlBlock.CalculateShipMass().BaseMass; // mass of the ship without cargo
double cargoMass = totalMass - baseMass; // mass of the cargo
double actualMass = baseMass + (cargoMass / InventoryMultiplier); // the mass the game uses for physics calculation
double shipWeight = actualMass * gravityStrength; // weight in newtons of the ship
double velocity = controlBlock.GetShipSpeed(); // ship velocity
double brakeDistance = CalculateBrakeDistance(gravityStrength, actualMass, altitude, thrustController.availableThrust, velocity);
double brakeAltitude = StopAltitude + brakeDistance; // at this altitude the ship will start slowing Down
if (Autopilot)
{
gyroController.Align(gravity);
if (input == "fall")
{
// This is a workaround to a game bug (ship speed greater than speed limit when free falling in natural gravity)
// Pros: your ship will not crash. Cons: you will waste a tiny amount of hydrogen.
thrustController.ApplyThrust(1);
}
if (altitude <= (brakeAltitude + AltitudeMargin))
{
// BRAKE!!!
thrustController.ApplyFullThrust(); // Maybe just enable dampeners
}
if (altitude <= (StopAltitude + DisableMargin + AltitudeMargin))
{
if (velocity < StopSpeed)
{
gyroController.Stop();
WriteLine("Autopilot deactivated (automatically)");
}
if (SmartDeactivation)
{
if (OldVelocity3D.X * velocity3D.X < 0 || OldVelocity3D.Y * velocity3D.Y < 0 || OldVelocity3D.Z * velocity3D.Z < 0)
{
gyroController.Stop();
WriteLine("Autopilot deactivated (automatically)");
}
}
}
}
OldVelocity3D = velocity3D;
if (input == "stop")
{
Runtime.UpdateFrequency = UpdateFrequency.None;
gyroController.Stop();
thrustController.Stop();
WriteLine("Autopilot deactivated (manually)");
}
}
private void TestMotionInput()
{
if (!EngineIsON)
{
return;
}
Vector3D moveInput = Cockpit.MoveIndicator;
//ToLog("\nmoveInput: " + VectToStr(moveInput), true);
InertiaDampeners = Cockpit.DampenersOverride;
//ToLog("\nDampeners: " + InertiaDampeners.ToString(), true);
Vector3D WorldSpeed = Cockpit.GetShipVelocities().LinearVelocity;
Vector3D WorldAngVel = Cockpit.GetShipVelocities().AngularVelocity;
Vector3D Gravity = Cockpit.GetNaturalGravity();
Vector3D normVertical = -Vector3D.Normalize(Gravity);
//матрица вращения в СО, связанную с вектором гравитации
if (!PlanetMatrixReady)
{
Vector3D normForward = normVertical.Cross(Cockpit.WorldMatrix.Right);
PlanetMatrix = MatrixD.CreateWorld(Vector3D.Zero, normForward, normVertical);
PlanetMatrixInv = MatrixD.Invert(PlanetMatrix);
PlanetMatrixReady = true;
}
Vector3D PlanetSpeed = Vector3D.Rotate(WorldSpeed, PlanetMatrixInv);
//ToLog("\nSpeed 3D: " + VectToStr(PlanetSpeed), true);
Vector3D PlanetAngularVelocity = Vector3D.Rotate(WorldAngVel, PlanetMatrixInv);
//test!!!
//Vector3D vForw = Vector3D.Rotate(Cockpit.WorldMatrix.Forward, PlanetMatrixInv);
//ToLog("\nForw. test: " + VectToStr(vForw), true);
Vector3D moveInputSign = Vector3D.Sign(moveInput);
double curAltitude, deltaAlt = 0;
Cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface, out curAltitude);
if (InertiaDampeners)
{
DesiredSpeed.X = MaxSpeed * moveInputSign.X;
if (AltitudeHoldMode)
{
deltaAlt = Math.Abs(DesiredAltitude) * 0.01;
if (deltaAlt < 0.1)
{
deltaAlt = 0.1;
}
if (moveInputSign.Y != 0.0)
{
DesiredAltitude += deltaAlt * moveInputSign.Y;
if (DesiredAltitude < 0.0)
{
DesiredAltitude = 0;
}
}
deltaAlt = DesiredAltitude - curAltitude;
//ускорение потом вычислим, желаемая скорость для этого не нужна
}
DesiredSpeed.Y = MaxSpeed * moveInputSign.Y;
if (CruiseControl)
{
DesiredSpeed.Z += DeltaSpeed * moveInputSign.Z;
}
else
{
DesiredSpeed.Z = MaxSpeed * moveInputSign.Z;
}
//DesiredSpeed = Vector3D.Sign(moveInput) * MaxSpeed;
}
else
{
//DesiredSpeed = PlanetSpeed;
if (moveInput.X == 0d)
{
DesiredSpeed.X = PlanetSpeed.X;
}
else
{
DesiredSpeed.X = MaxSpeed * moveInputSign.X;
}
if (moveInput.Y == 0d)
{
DesiredSpeed.Y = PlanetSpeed.Y;
}
else
{
DesiredSpeed.Y = MaxSpeed * moveInputSign.Y;
}
if (moveInput.Z == 0d)
{
DesiredSpeed.Z = PlanetSpeed.Z;
}
else
{
if (CruiseControl)
{
DesiredSpeed.Z += DeltaSpeed * moveInputSign.Z;
}
else
{
DesiredSpeed.Z = MaxSpeed * moveInputSign.Z;
}
}
}
DesiredSpeed = Vector3D.Clamp(DesiredSpeed, MaxSpeedMin, MaxSpeedMax);
if (CruiseControl)
{
ToLog("\nСкорость: " + (-PlanetSpeed.Z).ToString("#0") + " / " + (-DesiredSpeed.Z).ToString("#0"), true);
}
else
{
ToLog("\nСкорость: " + (-PlanetSpeed.Z).ToString("#0"), true);
}
if (InertiaDampeners && AltitudeHoldMode)
{
ToLog("\nВысота: " + curAltitude.ToString("#0") + " / " + DesiredAltitude.ToString("#0"), true);
}
else
{
ToLog("\nВысота: " + curAltitude.ToString("#0"), true);
}
Vector3D SpeedDeviation = DesiredSpeed - PlanetSpeed;
//ToLog("\nDes.spd: " + VectToStr(DesiredSpeed), true);
//ToLog("\nSpd.dev: " + VectToStr(SpeedDeviation), true);
//Z - вперед, X - вправо, Y - вверх
Vector3D DesiredAccelerations;
const double SigmoidFactorAlpha = 0.5;
DesiredAccelerations.X = Sigmoid(SpeedDeviation.X, SigmoidFactorAlpha) * CurrentRegimeAccel;
DesiredAccelerations.Z = Sigmoid(SpeedDeviation.Z, SigmoidFactorAlpha) * CurrentRegimeAccel;
if (AltitudeHoldMode && InertiaDampeners)
{
ToLog("\ndeltaAlt: " + deltaAlt.ToString("#0.000000"), true);
//ToLog("\nPlanetSpeed.Y: " + PlanetSpeed.Y.ToString("#0.000000"), true);
////нужно такое ускорение, чтобы корабль остановился при нулевом отклонении по высоте
//double absDeltaAlt = Math.Abs(deltaAlt);
//if (absDeltaAlt >= 0.5)
//{
// int signDeltaAlt = Math.Sign(deltaAlt);
// //минимальное расстояние, на котором мы еще успеем затормозить
// if (Math.Sign(PlanetSpeed.Y) != signDeltaAlt)
// {
// //удаляемся
// DesiredAccelerations.Y = CurrentRegimeAccel * signDeltaAlt;
// //ToLog("\nудаляемся", true);
// }
// else
// {
// //приближаемся, оценим тормозной путь
// double temp = 0.5 * PlanetSpeed.Y * PlanetSpeed.Y;
// double brakingDist = temp / CurrentRegimeAccel;
// //ToLog("\nbrakingDist: " + brakingDist.ToString("#0.00000"), true);
// if (absDeltaAlt > brakingDist)
// {
// //еще успеваем затормозить, поэтому даем максимальное ускорение
// DesiredAccelerations.Y = CurrentRegimeAccel * signDeltaAlt;
// }
// else
// {
// //уменьшаем ускорение
// DesiredAccelerations.Y = MathHelperD.Clamp(-temp / deltaAlt, -CurrentRegimeAccel, CurrentRegimeAccel);
// }
// }
//}
//else
//{
// DesiredAccelerations.Y = Sigmoid(SpeedDeviation.Y, SigmoidFactorAlpha) * CurrentRegimeAccel;
// //ToLog("\n!!!", true);
//}
DesiredAccelerations.Y = CalcAccelToStopAtPos(deltaAlt, PlanetSpeed.Y, CurrentRegimeAccel, 0.5, 0.5);
}
else
{
DesiredAccelerations.Y = Sigmoid(SpeedDeviation.Y, SigmoidFactorAlpha) * CurrentRegimeAccel;
}
ToLog("\nDes.acc: " + VectToStr(DesiredAccelerations), true);
//double PlanetElevation;
//Cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface, out PlanetElevation);
double MaxDownThrust = 0;
foreach (IMyThrust t in DownTrusters)
{
MaxDownThrust += t.MaxEffectiveThrust;
}
double ShipMass = Cockpit.CalculateShipMass().PhysicalMass;
//ToLog("\nMax thr: " + MaxDownThrust.ToString("#0.0"), true);
//ToLog("\nMass: " + ShipMass.ToString("#0.0"), true);
double DesiredVertivalAcc = DesiredAccelerations.Y + Gravity.Length();
if (DesiredVertivalAcc < 1.0)
{
DesiredVertivalAcc = 1.0;
}
ToLog("\nDes.vert.acc: " + DesiredVertivalAcc.ToString("#0.000"), true);
double cosGravityToUp = normVertical.Dot(Cockpit.WorldMatrix.Up);
double Thrust = ShipMass * DesiredVertivalAcc / cosGravityToUp;
//ToLog("\nThrust: " + Thrust.ToString("#0.000"), true);
//раздадим на движки
float ThrustFactor = (float)(Thrust / MaxDownThrust);
ToLog("\nThr.factor: " + ThrustFactor.ToString("#0.000"), true);
if (ThrustFactor > 1f)
{
ThrustFactor = 1f;
}
foreach (IMyThrust t in DownTrusters)
{
t.ThrustOverridePercentage = ThrustFactor;
}
//углы откладываются от вертикали, X - тангаж, Y - крен
Vector2D DesiredAngles;
DesiredAngles.X = Math.Atan2(DesiredAccelerations.Z, DesiredVertivalAcc);
DesiredAngles.Y = Math.Atan2(DesiredAccelerations.X, DesiredVertivalAcc);
DesiredAngles = Vector2D.Clamp(DesiredAngles, MaxDeviationAngleMin, MaxDeviationAngleMax);
ToLog("\nDes.angl: " + Vect2ToStr(DesiredAngles), true);
double cosGravityToForward = normVertical.Dot(Cockpit.WorldMatrix.Forward);
double cosGravityToRight = normVertical.Dot(Cockpit.WorldMatrix.Left);
//ToLog("\nCos(G) to F/R: " + cosGravityToForward.ToString("#0.000") + " / " + cosGravityToRight.ToString("#0.000"), true);
Vector2D CurrentAngles;
CurrentAngles.X = Math.Atan2(cosGravityToForward, cosGravityToUp);
CurrentAngles.Y = Math.Atan2(cosGravityToRight, cosGravityToUp);
ToLog("\nCur.angl: " + Vect2ToStr(CurrentAngles), true);
//ошибка угла
Vector2D AnglesDeviation = Vector2D.Clamp(NormAngles2D(DesiredAngles - CurrentAngles), MaxDeviationAngleMin, MaxDeviationAngleMax);
ToLog("\nAngl.dev: " + Vect2ToStr(AnglesDeviation, "#0.00000"), true);
Vector2D GyroSignals;
////демфируем сигнал при приближении отклонения к максимальному значению
//GyroSignals.X = SignalDamper(AnglesDeviation.X, MaxDeviationAngle, CurrentAngles.X, MaxDeviationAngle, 0.5);
//GyroSignals.Y = SignalDamper(AnglesDeviation.Y, MaxDeviationAngle, CurrentAngles.Y, MaxDeviationAngle, 0.5);
ToLog("\nPAV: " + VectToStr(PlanetAngularVelocity), true);
GyroSignals.X = CalcAccelToStopAtPos(AnglesDeviation.X, PlanetAngularVelocity.X, MaxAngularAcceleration, 0.5 * Math.PI / 180.0, 1);
GyroSignals.Y = CalcAccelToStopAtPos(AnglesDeviation.Y, -PlanetAngularVelocity.Z, MaxAngularAcceleration, 0.5 * Math.PI / 180.0, 1);
ToLog("\nGyroSignals: " + Vect2ToStr(GyroSignals, "#0.00000"), true);
//раздадим на гороскопы
GyroPitch = -(float)(GyroPithPID.Reaction(GyroSignals.X) * GyroFactor);
GyroRoll = (float)(GyroRollPID.Reaction(GyroSignals.Y) * GyroFactor);
//GyroPitch = -(float)(AnglesDeviation.X * GyroFactor);
//GyroRoll = (float)(AnglesDeviation.Y * GyroFactor);
ToLog("\nGyro P/Y/R: " + GyroPitch.ToString("#0.000") + " / " + GyroYaw.ToString("#0.000") + " / " + GyroRoll.ToString("#0.000"), true);
foreach (GyroDefinition g in Gyros)
{
SetGyroParams(g, true, GyroPitch, GyroYaw, GyroRoll);
}
}
public void Main(string argument)
{
//Обработка команд с контроллера.
DesiredElevation += RemCon.MoveIndicator.Y / 5;
DesiredForwardVelocity -= RemCon.MoveIndicator.Z / 5;
//Обработка аргументов.
if (argument != "")
{
switch (argument)
{
//Аргумент для полной остановки.
case "Stop":
{
DesiredForwardVelocity = 0;
break;
}
default:
break;
}
}
//Получаем и нормализуем вектор гравитации. Это наше направление "вниз" на планете.
Vector3D GravityVector = RemCon.GetNaturalGravity();
Vector3D GravNorm = Vector3D.Normalize(GravityVector);
Vector3D MyVelocityVector = RemCon.GetShipVelocities().LinearVelocity;
//получаем текущую высоту и вертикальную скорость и вес
RemCon.TryGetPlanetElevation(MyPlanetElevation.Surface, out CurrentElevation);
DeltaElevation = (float)(DesiredElevation - CurrentElevation);
float VerticalVelocity = -(float)MyVelocityVector.Dot(GravNorm);
// float Weight = (float)GravityVector.Length()*RemCon.CalculateShipMass().PhysicalMass;
//Защита крафта от падений.
//Защита от резкого роста высоты (при пролёте над оврагами, резкий спуск с горы).
//Защита от разряда баттарей.
foreach (IMyBatteryBlock bat in batList)
{
BatteryStored += bat.CurrentStoredPower;
BatteryMaxStored += bat.MaxStoredPower;
}
BatteryCharged = BatteryStored / (BatteryMaxStored / 100);
if (BatteryCharged <= 8)
{
DesiredElevation = 0;
}
//Вывод сервисной информации.
Display("", "infoLCD", true);
Display("Speed: " + DesiredForwardVelocity + "\r\n", "infoLCD");
Display("Height: " + DesiredElevation, "infoLCD");
Display("DeltaElevation: " + DeltaElevation, "infoLCD");
Display("BatteryCharged: " + BatteryCharged, "infoLCD");
//Считаем косинус угла наклона крафта
float TiltCos = (float)RemCon.WorldMatrix.Down.Dot(GravNorm);
//Считаем тягу
float Thrust = (float)((1 + (DeltaElevation * kV - VerticalVelocity) * kA) * GravityVector.Length() * RemCon.CalculateShipMass().PhysicalMass / TiltCos);
if (Thrust <= 0)
{
Thrust = 1;
}
/*
* float HoverCorrection = (DeltaElevation * kV - VerticalVelocity) * kA;
* float Thrust = (float)(GravityVector.Length() * RemCon.CalculateShipMass().PhysicalMass * (1 + HoverCorrection) / TiltCos);
* if (Thrust <= 0)
* Thrust = 1;
*
* Echo(""+ HoverCorrection + "\n" + TiltCos + "\n"+ thrList.Count+ "\n" + DeltaElevation + "\n" + VerticalVelocity); */
Vector3D MyVelocityVectorNorm = Vector3D.Reject(MyVelocityVector, RemCon.WorldMatrix.Forward) / 10;
if (MyVelocityVectorNorm.Length() > 1)
{
MyVelocityVectorNorm = Vector3D.Normalize(MyVelocityVectorNorm);
}
Vector3D ForwardInput = Vector3D.Normalize(Vector3D.Reject(RemCon.WorldMatrix.Forward, GravNorm)) * RemCon.MoveIndicator.Z;
Vector3D LeftInput = Vector3D.Normalize(Vector3D.Reject(RemCon.WorldMatrix.Left, GravNorm)) * RemCon.RollIndicator;
//Управляем скоростью
CurrentForwardVelocity = MyVelocityVector.Dot(Vector3D.Normalize(Vector3D.Reject(RemCon.WorldMatrix.Forward, GravNorm)));
ForwardAccel = CurrentForwardVelocity - OldCurrentForwardVelocity;
OldCurrentForwardVelocity = CurrentForwardVelocity;
double VelocityDelta = DesiredForwardVelocity - CurrentForwardVelocity;
double VelocityFactor = (VelocityDelta * Tilt_kV - ForwardAccel) * Tilt_kA;
Vector3D TiltCorrector = -Vector3D.Normalize(Vector3D.Reject(RemCon.WorldMatrix.Forward, GravNorm)) * VelocityFactor;
Vector3D AlignVector = Vector3D.Normalize(GravNorm + TiltCorrector + MyVelocityVectorNorm / 2 + (LeftInput) / 1.2);
//Получаем проекции вектора прицеливания на все три оси блока ДУ.
float PitchInput = -(float)AlignVector.Dot(RemCon.WorldMatrix.Forward);
float RollInput = (float)AlignVector.Dot(RemCon.WorldMatrix.Left);
//На рыскание просто отправляем сигнал рыскания с контроллера. Им мы будем управлять вручную.
float YawInput = RemCon.MoveIndicator.X;
//для каждого гироскопа устанавливаем текущие значения по тангажу, крену, рысканию.
foreach (IMyGyro gyro in gyroList)
{
gyro.GyroOverride = true;
gyro.Yaw = YawInput;
gyro.Roll = RollInput;
gyro.Pitch = PitchInput;
}
//раздаем тягу на движки
foreach (IMyThrust thr in thrList)
{
thr.ThrustOverride = Thrust / thrList.Count;
}
}
public void Update(double deltaTime)
{
if (!Enabled)
{
return;
}
//Reduces the target speed in relation to altitude.
double height;
double newTarget;
if (descending)
{
if (controller.TryGetPlanetElevation(MyPlanetElevation.Surface, out height))
{
if (height > TargetAltDescending)
{
newTarget = -((height - TargetAltDescending) / deceleration);
}
else
{
newTarget = ((TargetAltDescending - height) / deceleration);
}
if (newTarget < TargetSpeed)
{
AdaptiveTargetSpeed = TargetSpeed;
}
else
{
AdaptiveTargetSpeed = newTarget;
}
//screen.WriteText("\nGnd alt: " + height.ToString("n3"), true);
}
}
else
{
if (controller.TryGetPlanetElevation(elevationType, out height))
{
if (height > TargetAltAcending)
{
newTarget = -((height - TargetAltAcending) / deceleration);
}
else
{
newTarget = ((TargetAltAcending - height) / deceleration);
}
if (newTarget > TargetSpeed)
{
AdaptiveTargetSpeed = TargetSpeed;
}
else
{
AdaptiveTargetSpeed = newTarget;
}
//screen.WriteText("\nSea alt: " + height.ToString("n3"), true);
}
else
{
//Failed at getting planet elevation, means we're not in gravity well.
if (DisableOnNaturalGravityExit && startedInNaturalGravity)
{
Stop();
return;
}
}
}
//Mass/Gravity
double mass = controller.CalculateShipMass().PhysicalMass;
double gravity = Vector3D.Dot(controller.GetNaturalGravity(), controller.WorldMatrix.GetOrientation().Down); //TODO: Is this supposed to be down or dependent on selected thrusters?
//mass *= 9.80665f; //Convert to newton.
double weight = mass * gravity;
//Speed
var vel = controller.GetShipVelocities().LinearVelocity;
if (forward == Base6Directions.Direction.Up)
{
Speed = (Vector3D.TransformNormal(vel, MatrixD.Transpose(controller.WorldMatrix))).Y;
}
else if (forward == Base6Directions.Direction.Down)
{
Speed = -(Vector3D.TransformNormal(vel, MatrixD.Transpose(controller.WorldMatrix))).Y;
}
else if (forward == Base6Directions.Direction.Forward)
{
Speed = -(Vector3D.TransformNormal(vel, MatrixD.Transpose(controller.WorldMatrix))).Z;
}
else if (forward == Base6Directions.Direction.Backward)
{
Speed = (Vector3D.TransformNormal(vel, MatrixD.Transpose(controller.WorldMatrix))).Z;
}
double difference = AdaptiveTargetSpeed - Speed;
double errorMagnitude = MathHelper.Clamp(difference / 5, -1, 1); //More than 5 away from target = full thrust.
float electricThrust = GetElectricThrust(forward);
float electricThrustRev = GetElectricThrust(reverse);
float hydroThrust = GetHydrogenThrust(forward);
float hydroThrustRev = GetHydrogenThrust(reverse);
//TODO: Calulate thrust needed, tanking speed into account. MAss isnt just kg * gravity when hurling towards earth. Need how many newtons it takes to reach a specific speed, 0 in this case.
//That should work for liftoff as well.
//a = (v1 - v0) / t
//F = m * a
//Time is 1.
//F = m * (v1 - v0)
//double thrustNeeded = mass * difference;
//This is not more stable than what I did before.
//screen.WriteText("\nTS: " + AdaptiveTargetSpeed.ToString("n3") + ", S: " + Speed.ToString("n3"), true);
//screen.WriteText("\nerr " + errorMagnitude.ToString("n3"), true);
////screen.WriteText("\nneed " + thrustNeeded.ToString("n3"), true);
//screen.WriteText("\nthr " + (electricThrust + hydroThrust).ToString("n3"), true);
//Dividing by zero will happen here, but since it's float it will result in infinity instead of exception, which is fine.
float thrustOverride = MathHelper.Clamp((float)weight / electricThrust, 0, 1); //Convert to percent
float hydroThrustOverride = MathHelper.Clamp(((float)weight - electricThrust) / hydroThrust, 0, 1);
float thrustOverrideRev = 0;
float hydroThrustOverrideRev = 0;
float thrustExcess = 0;
float hydroThrustExcess = 0;
thrustExcess = 1 - thrustOverride;
hydroThrustExcess = 1 - hydroThrustOverride;
if (Speed < AdaptiveTargetSpeed)
{
thrustOverride += thrustExcess * (float)errorMagnitude;
hydroThrustOverride += hydroThrustExcess * (float)errorMagnitude;
}
else
{
//thrustOverride *= 1 - (float)errorMagnitude;
//hydroThrustOverride *= 1 - (float)errorMagnitude;
thrustOverride *= 1 + (float)errorMagnitude;
hydroThrustOverride *= 1 + (float)errorMagnitude;
}
thrustOverrideRev = -MathHelper.Clamp((electricThrustRev * (float)errorMagnitude) / electricThrustRev, -1, 0);
hydroThrustOverrideRev = -MathHelper.Clamp((hydroThrustRev * (float)errorMagnitude) / hydroThrustRev, -1, 0);
//Not using h2 thruster if setting says so
//if (electricThrust > mass) hydroThrustOverride = 0.000001f;
//DEBUGLCD?.WritePublicText($"mass kg {mass2}\nmass n {mass.ToString("n3")}\ne thrust {thrust.ToString("n3")}\nh thrust {hydroThrust.ToString("n3")}\ne override {thrustOverride.ToString("n3")}\nh override {hydroThrustOverride.ToString("n3")}\ne excess {thrustExcess.ToString("n3")}\nh excess {hydroThrustExcess.ToString("n3")}\ndiff {difference.ToString("n3")}\nerr {errorMagnitude.ToString("n3")}\ngrav {gravity.ToString("n3")}\nelevation {elevation.ToString("n3")}");
//Prevent setting thrust override to 0, as that turns it off.
if (thrustOverride <= 0 || double.IsNaN(thrustOverride))
{
thrustOverride = 0.000001f;
}
if (hydroThrustOverride <= 0 || double.IsNaN(hydroThrustOverride))
{
hydroThrustOverride = 0.000001f;
}
if (thrustOverrideRev <= 0 || double.IsNaN(thrustOverrideRev))
{
thrustOverrideRev = 0.000001f;
}
if (hydroThrustOverrideRev <= 0 || double.IsNaN(hydroThrustOverrideRev))
{
hydroThrustOverrideRev = 0.000001f;
}
ThrustOverrideElectric = 0;
for (int i = 0; i < electricThrusters[forward].Count; i++)
{
if (electricThrusters[forward][i].MaxEffectiveThrust / electricThrusters[forward][i].MaxThrust <= cutoff)
{
electricThrusters[forward][i].ThrustOverridePercentage = 0.000001f; //If max effective trhust is less than 5%, don't use the thruster
}
else
{
electricThrusters[forward][i].ThrustOverridePercentage = thrustOverride;
ThrustOverrideElectric += thrustOverride;
}
}
for (int i = 0; i < hydrogenThrusters[forward].Count; i++)
{
hydrogenThrusters[forward][i].ThrustOverridePercentage = hydroThrustOverride;
}
//screen.WriteText("\novr-rev " + thrustOverrideRev.ToString("n3"), true);
//screen.WriteText("\novr-hrev " + hydroThrustOverrideRev.ToString("n3"), true);
for (int i = 0; i < electricThrusters[reverse].Count; i++)
{
if (electricThrusters[reverse][i].MaxEffectiveThrust / electricThrusters[reverse][i].MaxThrust <= cutoff)
{
electricThrusters[reverse][i].ThrustOverridePercentage = 0.000001f; //If max effective trhust is less than 5%, don't use the thruster
}
else
{
electricThrusters[reverse][i].ThrustOverridePercentage = thrustOverrideRev;
ThrustOverrideElectric += thrustOverrideRev;
}
}
for (int i = 0; i < hydrogenThrusters[reverse].Count; i++)
{
hydrogenThrusters[reverse][i].ThrustOverridePercentage = hydroThrustOverrideRev;
}
ThrustOverrideElectric = ThrustOverrideElectric / (electricThrusters[forward].Count + electricThrusters[reverse].Count);
ThrustOverrideHydrogen = hydroThrustOverride + hydroThrustOverrideRev;
//screen.WriteText("\nTS: " + AdaptiveTargetSpeed.ToString("n3") + ", S: " + speed.ToString("n3"), true);
//screen.WriteText("\nE: " + electricThrusters[forward].Count + ", ovr " + thrustOverride.ToString("n3") + ", thr " + electricThrust.ToString("n3"), true);
//screen.WriteText("\nH " + hydrogenThrusters[forward].Count + ", ovr " + hydroThrustOverride.ToString("n3") + ", thr " + hydroThrust.ToString("n3"), true);
//screen.WriteText("\nerr " + errorMagnitude.ToString("n3"), true);
}
