Beispiel #1
0
 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());
 }