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()); }