public AltitudeController(Vessel vessel, IVertSpeedController vertSpeedController)
{
Target = new AltitudeTarget(vessel);
ControlElement = new VertSpeedElement(vertSpeedController);
SetPoint = 2000.0f;
CoeffP = 0.5f;
TimeConstI = 10.0f;
UseCoeffI = false;
}
public string Update()
{
massOfShip = 0;
velocityTowardGravity = 0;
emergencyThrust = 0;
normalThrust = 0;
gravThrust = 0;
seaLevelAltitude = 0;
surfaceAltitude = 0;
lidarAltitude = 0;
landingAltitude = 0;
StatusBuilder.Clear();
StatusBuilder.Append($"\n OrbitalMode: {OrbitalMode}");
if (OrbitalMode == OrbitalOperation.Off)
{
return(StatusBuilder.ToString());
}
if (shipControllers.Any(de => de != de.CubeGrid.GetCubeBlock(de.Position)?.FatBlock) ||
thrusters.Any(de => de != de.CubeGrid.GetCubeBlock(de.Position)?.FatBlock) ||
emergencyThrusters.Any(de => de != de.CubeGrid.GetCubeBlock(de.Position)?.FatBlock) ||
gravDriveNeg.Any(de => de != de.CubeGrid.GetCubeBlock(de.Position)?.FatBlock) ||
gravDrivePos.Any(de => de != de.CubeGrid.GetCubeBlock(de.Position)?.FatBlock) ||
vMass.Any(de => de != de.CubeGrid.GetCubeBlock(de.Position)?.FatBlock) ||
downCameras.Any(de => de != de.CubeGrid.GetCubeBlock(de.Position)?.FatBlock))
{
OrbitalListNeedsBuilding = true;
}
if (!OrbitalListNeedsBuilding && shipControllers.Count > 0)
{
gravityVector = shipControllers[0].GetNaturalGravity();
normalGravity = gravityVector.Length() / 9.81;
if (thrusters.Count > 0 && thrusters[0].WorldMatrix.GetClosestDirection(gravityVector) == Base6Directions.Direction.Forward)
{
OrbitalListNeedsBuilding = true;
}
}
if (shipControllers.Count == 0 || OrbitalListNeedsBuilding)
{
foreach (IMyThrust de in thrusters)
{
de?.SetValueFloat("Override", 0);
}
foreach (IMyThrust de in emergencyThrusters)
{
de?.SetValueFloat("Override", 0);
}
foreach (IMyGravityGenerator de in gravDrivePos)
{
de.GravityAcceleration = 0;
}
foreach (IMyGravityGenerator de in gravDriveNeg)
{
de.GravityAcceleration = 0;
}
BuildOrbitList();
}
if (shipControllers.Count == 0)
{
//cannot go on.
StatusBuilder.Append("\nOrbital: Error no shipController");
return(StatusBuilder.ToString());
}
massOfShip = shipControllers[0].CalculateShipMass().PhysicalMass;
shipVector = shipControllers[0].GetShipVelocities().LinearVelocity;
shipToGravityVector = VectorProjection(shipVector, gravityVector);
velocityTowardGravity = ((int)(-1 * Math.Sign(gravityVector.Dot(shipVector)))) * shipToGravityVector.Length();
StatusBuilder.Append($"\n Mass : {DisplayLargeNumber(massOfShip)} kg");
StatusBuilder.Append($"\n Rate of Change: {velocityTowardGravity.ToString("0.00")} m/s");
shipControllers[0].TryGetPlanetElevation(MyPlanetElevation.Sealevel, out seaLevelAltitude);
foreach (IMyShipController de in shipControllers)
{
de.TryGetPlanetElevation(MyPlanetElevation.Surface, out tempDouble);
if (tempDouble > surfaceAltitude)
{
surfaceAltitude = tempDouble;
}
}
//thrusters
foreach (IMyThrust de in thrusters)
{
if (de.IsWorking)
{
normalThrust += de.MaxEffectiveThrust;
}
}
if (normalThrust == 0)
{
normalThrust = 1;
}
foreach (IMyThrust de in emergencyThrusters)
{
emergencyThrust += de.MaxEffectiveThrust;
}
if (GravDriveOn)
{
gravThrust = 490500 * (gravDriveNeg.Count + gravDrivePos.Count);
}
thrustNeeded = (float)(massOfShip * normalGravity * 9.81);
StatusBuilder.Append($"\n Thrust needed : {DisplayLargeNumber(thrustNeeded)} N");
//Decision point
if (normalGravity == 0)
{
foreach (IMyThrust de in thrusters)
{
de.SetValueFloat("Override", 0);
}
foreach (IMyThrust de in emergencyThrusters)
{
de.SetValueFloat("Override", 0);
de.Enabled = false;
}
foreach (IMyGravityGenerator de in gravDriveNeg)
{
de.GravityAcceleration = 0;
}
foreach (IMyGravityGenerator de in gravDrivePos)
{
de.GravityAcceleration = 0;
}
StatusBuilder.Append($"\n Not in gravity well");
return(StatusBuilder.ToString());
}
effectiveGravDriveThrust = gravThrust * MathHelper.Clamp(1 - 2 * normalGravity, 0f, 1f);
maxEffectiveThrust = effectiveGravDriveThrust + normalThrust;
tempAdjustmentGravity = MathHelper.Clamp(normalGravity * 9.31, 0, VelocityLimit / 2);
availableThrust = Math.Max(maxEffectiveThrust - thrustNeeded, 0);
targetVelocity = 0;
if (OrbitalMode == OrbitalOperation.AltitudeMode || OrbitalMode == OrbitalOperation.HoverMode)
{
foreach (IMyCameraBlock de in downCameras)
{
de.EnableRaycast = true;
}
lidarAltitude = LidarRange(downCameras, 0, 0);
if (lidarAltitude < surfaceAltitude)
{
landingAltitude = lidarAltitude;
}
else
{
landingAltitude = surfaceAltitude;
}
if (OrbitalMode == OrbitalOperation.HoverMode)
{
tempDouble = HoverTarget + seaLevelAltitude - landingAltitude;
}
else
{
tempDouble = AltitudeTarget;
}
if (seaLevelAltitude < tempDouble - AltitudeBuffer)
{
targetVelocity = MathHelper.Clamp(Math.Sqrt(tempAdjustmentGravity * Math.Abs(tempDouble - seaLevelAltitude - velocityTowardGravity)), 0, VelocityLimit);
}
else if (seaLevelAltitude > tempDouble + AltitudeBuffer)
{
if (landingAltitude < (HeightOffset + 100 + velocityTowardGravity * velocityTowardGravity / tempAdjustmentGravity))
{
//landing
targetVelocity = -Math.Max(Math.Sqrt(Math.Abs(MathHelper.Clamp((landingAltitude - HeightOffset - 12.5), 0d, 100) * tempAdjustmentGravity)), 1d);
}
else
{
targetVelocity = -MathHelper.Clamp(Math.Sqrt(0.7 * tempAdjustmentGravity * Math.Abs(seaLevelAltitude - tempDouble + velocityTowardGravity)), 0, VelocityLimit);
}
}
}
//could just do else here
if (OrbitalMode == OrbitalOperation.GravityMode)
{
if (Math.Round(normalGravity, 2) > Math.Round(GravityTarget, 2))
{
if (VelocityLimit > tempAdjustmentGravity * 10)
{
targetVelocity = 10 * tempAdjustmentGravity;
}
else
{
targetVelocity = VelocityLimit;
}
}
else if (Math.Round(normalGravity, 2) < Math.Round(GravityTarget, 2))
{
targetVelocity = -VelocityLimit;
}
}
StatusBuilder.Append($"\n request velocity {targetVelocity.ToString("0.00")} m/s");
requestedThrust = (targetVelocity - velocityTowardGravity) * massOfShip + thrustNeeded;
//gravDrive
if (GravDriveOn)
{
thrustApplied = Math.Sign(requestedThrust) * Math.Min(Math.Abs(requestedThrust), effectiveGravDriveThrust);
requestedThrust -= Math.Min(Math.Abs(requestedThrust), effectiveGravDriveThrust);
}
requestedGravity = (float)((thrustApplied / (MathHelper.Clamp(1 - 2 * normalGravity, 0f, 1f)) / 50000) / (gravDrivePos.Count + gravDriveNeg.Count));
if (requestedGravity < 0 || float.IsNaN(requestedGravity))
{
requestedGravity = 0;
}
foreach (IMyGravityGenerator de in gravDrivePos)
{
de.GravityAcceleration = requestedGravity;
}
foreach (IMyGravityGenerator de in gravDriveNeg)
{
de.GravityAcceleration = -requestedGravity;
}
//Normal Drive
if (thrusters.Count > 0 && requestedThrust > 0)
{
thrustApplied = Math.Min(requestedThrust, normalThrust);
requestedThrust -= thrustApplied;
}
normalThrustPerThruster = (float)(100 * thrustApplied / normalThrust);
if (normalThrustPerThruster < 1.0001)
{
normalThrustPerThruster = 1.0001f;
}
foreach (IMyThrust de in thrusters)
{
de.SetValueFloat("Override", normalThrustPerThruster);
}
if (OrbitalMode == OrbitalOperation.GravityMode && normalGravity > GravityEmergency)
{
// Emergency mode
StatusBuilder.Append("\n Emergency thrust engaged");
foreach (IMyThrust de in emergencyThrusters)
{
de.Enabled = true;
de.SetValueFloat("Override", 100);
}
}
else if (OrbitalMode == OrbitalOperation.GravityMode && normalGravity < GravityEmergency)
{
foreach (IMyThrust de in emergencyThrusters)
{
de.Enabled = false;
de.SetValueFloat("Override", 0);
}
}
if (OrbitalMode == OrbitalOperation.AltitudeMode || OrbitalMode == OrbitalOperation.HoverMode)
{
StatusBuilder.Append($"\n Altitude = {seaLevelAltitude.ToString("0")} m.");
if (OrbitalMode == OrbitalOperation.AltitudeMode)
{
StatusBuilder.Append($" Target = {AltitudeTarget.ToString("0")} m.");
}
else
{
StatusBuilder.Append($" Hover at = {HoverTarget.ToString("0")} m.");
}
StatusBuilder.Append($"\n Ground level = {surfaceAltitude.ToString("0")} m.");
StatusBuilder.Append($"\n Lidar Altitude = {lidarAltitude.ToString("0")} m.");
}
if (OrbitalMode == OrbitalOperation.GravityMode)
{
StatusBuilder.Append($"\n Current Orbit = {normalGravity.ToString("0.000")} g.");
StatusBuilder.Append($"\n Target Orbit = {GravityTarget.ToString("0.000")} g.");
}
StatusBuilder.Append($"\n Lift: Normal: {DisplayLargeNumber((float)(normalThrust / (normalGravity * 9.81)))} kg.");
StatusBuilder.Append($"\n GravDrive: {DisplayLargeNumber((float)(effectiveGravDriveThrust / (normalGravity * 9.81)))} kg.");
StatusBuilder.Append($"\n Emergency: {DisplayLargeNumber((float)(emergencyThrust / (normalGravity * 9.81)))} kg.");
return(StatusBuilder.ToString());
}