public static Planet GetBestGuessFromInitialData(IMyShipController remote)
{
if (!double.IsNaN(remote.GetNaturalGravity().Length()))
{
Planet p = new Planet();
Vector3D center;
remote.TryGetPlanetPosition(out center);
p.Center = center;
double curdist = Vector3D.Distance(center, remote.GetPosition());
double sealevelelevation;
double curgrav = remote.GetNaturalGravity().Length();
remote.TryGetPlanetElevation(MyPlanetElevation.Sealevel, out sealevelelevation);
p.Radius = curdist - sealevelelevation;
p.GravityMathNumber = curgrav * Math.Pow(curdist, p.GravityExponent);
if (curdist > p.Radius * (1 + p.HillParameter))
{
double mathvalue = curgrav * Math.Pow(curdist, p.GravityExponent);
p.SurfaceGravity = mathvalue / Math.Pow(p.Radius * p.HillParameter + p.Radius, p.GravityExponent);
}
else
{
p.SurfaceGravity = curgrav;
}
return(p);
}
return(null);
}
public double GetAltitude()
{
double res = 0.0;
sc.TryGetPlanetElevation(MyPlanetElevation.Sealevel, out res);
return(res + 1550);
}
public bool Update()
{
if (Controller != null)
{
if (Controller.TryGetPlanetElevation(MyPlanetElevation.Sealevel, out val))
{
if (val != Value)
{
Value = val;
return(true);
}
}
}
return(false);
}
private bool CheckBrake()
{
//---Get speed
double currentSpeed = referenceBlock.GetShipSpeed();
double distanceToEnd = GetDistanceToTarget();
if (distanceToEnd == 0D)
{
return(true);
}
Vector3D shipVelocityVec = referenceBlock.GetShipVelocities().LinearVelocity;
Debug("currentSpeed", currentSpeed);
Debug("distanceToEnd", distanceToEnd);
Debug("shipVelocityVec", shipVelocityVec);
//--- Manage gravity
Vector3D gravityVec = referenceBlock.GetNaturalGravity();
double gravityVecMagnitude = gravityVec.Length();
if (gravityVecMagnitude > 0.0001)
{
double distanceToSurface;
referenceBlock.TryGetPlanetElevation(MyPlanetElevation.Surface, out distanceToSurface);
// TODO take into account this distance if we fly near the surface
}
distanceToEnd -= shipCenterToEdge;
float distanceToStartBraking = (float)GetBrakingDistanceThreshold((float)gravityVecMagnitude, shipVelocityVec);
Debug("distanceToStartBraking", distanceToStartBraking);
//Debug("distanceToStartStabilize", distanceToStartStabilize);
bool shouldBrake = distanceToEnd <= distanceToStartBraking;
if (distanceToEnd < 100 && currentSpeed < 1)
{
timeSpentStationary += 1;
}
else
{
timeSpentStationary = 0;
}
return(shouldBrake);
}
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();
}
}
}
private void InitModule()
{
Cockpit = GridTerminalSystem.GetBlockWithName(CockpitName) as IMyShipController;
GridTerminalSystem.GetBlocksOfType <IMyThrust>(DownTrusters);
List <IMyGyro> AllGyros = new List <IMyGyro>();
GridTerminalSystem.GetBlocksOfType <IMyGyro>(AllGyros);
SetupGyros(Cockpit, AllGyros, Gyros);
DebugLCD = GridTerminalSystem.GetBlockWithName(DebugLCDName) as IMyTextPanel;
InertiaDampeners = Cockpit.DampenersOverride;
Cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface, out DesiredAltitude);
SetEngineMode(0);
Runtime.UpdateFrequency = UpdateFrequency.Update10;
}
public void Update()
{
reference.TryGetPlanetElevation(MyPlanetElevation.Surface, out Elevation);
var controllerMatrix = reference.WorldMatrix;
var velocities = reference.GetShipVelocities();
//gravity
var gravityVec = reference.GetNaturalGravity();
GravityLength = (float)gravityVec.Length();
var referenceForward = controllerMatrix.Forward;
var referenceLeft = controllerMatrix.Left;
var referenceUp = controllerMatrix.Down;
Gravity.X = -(float)Math.Atan2(gravityVec.Dot(referenceForward), gravityVec.Dot(referenceUp));
Gravity.Z = (float)Math.Atan2(gravityVec.Dot(referenceLeft), gravityVec.Dot(referenceUp));
AvgGravity += (Gravity - AvgGravity) * .2f;
//aligned velocity
var velocityVector = velocities.LinearVelocity;
AlignedVelocity.X = (float)velocityVector.Dot(referenceLeft);
AlignedVelocity.Y = (float)velocityVector.Dot(referenceUp);
AlignedVelocity.Z = (float)velocityVector.Dot(referenceForward);
//grav aligned velocity
var matrix = Matrix.CreateFromYawPitchRoll(Gravity.X, -Gravity.Z, 0); //magic
AlignedToGravityVelocity = Vector3.Transform(AlignedVelocity, matrix);
var d = referenceForward.Cross(gravityVec);
if (reference.RotationIndicator.Y != 0)
{ // update expected direction only if user turn rover with mouse
LastDirection = CurrentDirection;
}
CurrentDirection = (float)Math.Atan2(d.X, d.Y);
}
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 Main(string argument, UpdateType updateSource)
{
if (!ModuleInited())
{
return;
}
//проверим аргументы
if (!string.IsNullOrEmpty(argument))
{
switch (argument)
{
case "power":
{
if (EngineIsON)
{
//выключаем движок
SetEngineMode(0);
}
else
{
//включаем движок
SetEngineMode(1);
}
break;
}
case "power-":
{
SetEngineMode(EngineMode - 1);
break;
}
case "power+":
{
SetEngineMode(EngineMode + 1);
break;
}
case "stop":
DesiredSpeed.Z = 0;
break;
case "max":
DesiredSpeed.Z = -MaxSpeed;
break;
case "cruisecontrol":
CruiseControl = !CruiseControl;
break;
case "altitudehold":
AltitudeHoldMode = !AltitudeHoldMode;
if (AltitudeHoldMode)
{
Cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface, out DesiredAltitude);
}
break;
default: break;
}
}
ToLog("Engine mode: " + EngineMode.ToString());
InertiaDampeners = Cockpit.DampenersOverride;
TestRotationInput();
TestMotionInput();
}
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);
}
public void Main(string argument, UpdateType updateSource)
{
if (!ModuleInited())
{
return;
}
if (init_error)
{
Echo(init_message);
ProgBlockLCD.WriteText(init_message);
if (LCD != null)
{
LCD.WriteText(init_message);
}
return;
}
ToLog("", false, false);
Vector3D GravityVector = Cockpit.GetNaturalGravity();
Vector3D GravityNorm = Vector3D.Normalize(GravityVector);
Vector3D VelocityVector = Cockpit.GetShipVelocities().LinearVelocity;
Vector3D VelocityVectorNorm = Vector3D.Normalize(VelocityVector);
Vector3D ForwardVectorNorm = Vector3D.Normalize(Vector3D.Reject(Cockpit.WorldMatrix.Forward, GravityNorm));
Vector3D LeftVectorNorm = Vector3D.Normalize(Vector3D.Reject(Cockpit.WorldMatrix.Left, GravityNorm));
Vector3D ForwardInput = ForwardVectorNorm * Cockpit.MoveIndicator.Z;
Vector3D LeftInput = LeftVectorNorm * Cockpit.MoveIndicator.X;
Vector3D ForwardVelocity = Vector3D.ProjectOnVector(ref VelocityVector, ref ForwardVectorNorm);
Vector3D LeftVelocity = Vector3D.ProjectOnVector(ref VelocityVector, ref LeftVectorNorm);
float UpInput = Cockpit.MoveIndicator.Y / 10;
Vector3D AlignVector = Vector3D.Zero;
/*if (!string.IsNullOrEmpty(argument))
* {
* string cmd, cmd_param;
* string argument_norm = argument.Trim().ToLower();
* int spc_idx = argument_norm.IndexOf(" ");
* if (spc_idx == -1)
* {
* cmd = argument_norm;
* cmd_param = string.Empty;
* }
* else
* {
* cmd = argument_norm.Substring(0, spc_idx);
* cmd_param = argument_norm.Substring(spc_idx + 1).TrimStart();
* }*/
switch (argument)
{
case "Dampeners":
{
PilotMode = PilotModeEnum.Dampeners;
break;
}
case "Cruise":
{
PilotMode = PilotModeEnum.Cruise;
if (ForwardVectorNorm == Vector3D.Normalize(ForwardVelocity))
{
cruisespeed = -(float)ForwardVelocity.Length();
}
else
{
cruisespeed = (float)ForwardVelocity.Length();
}
break;
}
case "FreeMove":
{
PilotMode = PilotModeEnum.FreeMove;
break;
}
case "Drill":
{
PilotMode = PilotModeEnum.Cruise;
cruisespeed = 3f;
altitude = 4f;
foreach (IMyThrust hover in HoverList)
{
hover.SetValue <float>("altitudemin_slider_L", altitude);
}
break;
}
case "Down":
{
PilotMode = PilotModeEnum.Cruise;
cruisespeed = 5f;
altitude = 5f;
foreach (IMyThrust hover in HoverList)
{
hover.SetValue <float>("altitudemin_slider_L", altitude);
}
break;
}
default:
break;
}
//}
if (!Vector3D.IsZero(LeftInput))
{
AlignVector += LeftInput * KI;
}
else
if (PilotMode != PilotModeEnum.FreeMove)
{
AlignVector += LeftVelocity * KV;
}
if (!Vector3D.IsZero(ForwardInput))
{
if (PilotMode == PilotModeEnum.Cruise)
{
cruisespeed += -Cockpit.MoveIndicator.Z / 10;
AlignVector += (ForwardVelocity - ForwardVectorNorm * cruisespeed) * KV;
}
else
{
AlignVector += ForwardInput * KI;
}
}
else
if (PilotMode == PilotModeEnum.Cruise)
{
AlignVector += (ForwardVelocity - ForwardVectorNorm * cruisespeed) * KV;
}
else
if (PilotMode != PilotModeEnum.FreeMove)
{
AlignVector += ForwardVelocity * KV;
}
AlignVector += GravityNorm;
float PitchInput = -(float)AlignVector.Dot(Cockpit.WorldMatrix.Forward);
float RollInput = (float)AlignVector.Dot(Cockpit.WorldMatrix.Left);
float YawInput = Cockpit.RollIndicator;
foreach (IMyThrust hover in HoverList)
{
altitude = hover.GetValue <float>("altitudemin_slider_L");
altitude += UpInput;
hover.SetValue <float>("altitudemin_slider_L", altitude);
}
foreach (IMyGyro gyro in GyroList)
{
gyro.GyroOverride = true;
gyro.Yaw = YawInput;
gyro.Roll = RollInput;
gyro.Pitch = PitchInput;
}
DisplayUpdateCounter += 1;
if (DisplayUpdateCounter >= DisplayUpdateInterval)
{
ProgBlockLCD.WriteText("Cycle time, µs: " + (runTime * DisplayUpdateFreq).ToString("#0.0") + "\n");
Echo("Cycle time, µs: " + (runTime * DisplayUpdateFreq).ToString("#0.0") + "\n" + init_message);
DisplayUpdateCounter = 0;
runTime = 0d;
Cockpit.TryGetPlanetElevation(MyPlanetElevation.Sealevel, out SealevelAlt);
Cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface, out SurfaceAlt);
ProgBlockLCD.WriteText("Режим: " + PilotModeToString() + "\n", true);
ProgBlockLCD.WriteText(string.Format("cruisespeed: {0:#0.0}\n", cruisespeed), true);
ProgBlockLCD.WriteText(string.Format("FV: {0:#0.0} / LV: {1:#0.0} / V: {2:#0.0}\n", ForwardVelocity.Length(), LeftVelocity.Length(), VelocityVector.Length()), true);
ProgBlockLCD.WriteText(string.Format("Alt: {0:#0.0} / SLA: {1:#0.0} / SFA: {2:#0.0}\n", altitude, SealevelAlt, SurfaceAlt), true);
if (LCD != null)
{
string t = ProgBlockLCD.GetText();
LCD.WriteText(t);
}
}
runTime += Runtime.LastRunTimeMs;
}
//private void Main(string Package)
//{
// try
// {
// if (!Init)
// {
// Echo("Initializing:: ...");
// if (IsDirector)
// {
// Runtime.UpdateFrequency = UpdateFrequency.Update10;
// }
// else
// {
// Runtime.UpdateFrequency = UpdateFrequency.None;
// }
// // Load Custom Data
// if (Me.CustomData != null)
// {
// Echo("Loading Custom Data...");
// CustomData _CstmData = new CustomData(Me);
// string _ReadValue = null;
// // Common 共通
// _ReadValue = null;
// _ReadValue = _CstmData.LoadCustomData("Channel");
// if (_ReadValue != null)
// {
// Channel = _ReadValue;
// }
// _ReadValue = null;
// _ReadValue = _CstmData.LoadCustomData("IsDirector");
// if (_ReadValue != null)
// {
// IsDirector = (_ReadValue == "true");
// }
// _ReadValue = null;
// _ReadValue = _CstmData.LoadCustomData("ControlFactor");
// if (_ReadValue != null)
// {
// double _dCFactor = 0;
// if (double.TryParse(_ReadValue, out _dCFactor))
// {
// ControlFactor = _dCFactor;
// }
// }
// if (IsDirector)
// {
// // Director 旗艦
// _ReadValue = null;
// _ReadValue = _CstmData.LoadCustomData("Dist");
// if (_ReadValue != null)
// {
// double _dDist = 0;
// if (double.TryParse(_ReadValue, out _dDist))
// {
// Dist = _dDist;
// }
// }
// }
// else
// {
// // Drone ドローン
// _ReadValue = null;
// _ReadValue = _CstmData.LoadCustomData("DroneNum");
// if (_ReadValue != null)
// {
// int _iDroneNum = 0;
// if (int.TryParse(_ReadValue, out _iDroneNum))
// {
// DroneNum = _iDroneNum;
// }
// }
// _ReadValue = null;
// _ReadValue = _CstmData.LoadCustomData("AllowAttack");
// if (_ReadValue != null)
// {
// IsDirector = (_ReadValue == "true");
// }
// }
// }
// Echo("Initialized.");
// Init = true;
// }
// if (GatherCounter % GatherInterval == 0)
// {
// if (!ClearAndGather())
// {
// return;
// }
// GatherCounter = 0;
// }
// GatherCounter++;
// if (IsDirector)
// {
// if (!Package.Contains(":"))
// {
// string Argument = Package;
// Director(Argument);
// }
// }
// else
// {
// if (Package.Contains(":"))
// {
// Recieve(Package);
// }
// }
// }
// catch (Exception EX)
// {
// Echo("Exception at site: " + EX.TargetSite.ToString());
// Echo(EX.Message);
// //Log Report
// //if (ReportLog)
// //{
// // Echo(Log);
// //}
// if (PreventFurtherRuns)
// {
// throw (EX);
// }
// else
// {
// Echo(EX.Message);
// ClearAndGather();
// }
// }
//}
private void Director(string Argument)
{
Echo("Running Director Method");
DroneCount++;// Add 1 because 0 based index (director is 0);
if (Argument.Contains("T."))
{
SendTriggerCommand(Argument);
return;
}
if (Argument.Contains("M.Ref"))
{
SetReferencePosition(Argument);
return;
}
if (Argument.Contains("M.Dist"))
{
Echo("Distance Changed");
char[] digits = Command.Where(c => char.IsDigit(c)).ToArray();
Dist = Int32.Parse(digits.ToString());
}
// Declare Vectors
Vector3D Forward = MainController.WorldMatrix.Forward * Dist;
Vector3D Back = MainController.WorldMatrix.Backward * Dist;
Vector3D Right = MainController.WorldMatrix.Right * Dist;
Vector3D Left = MainController.WorldMatrix.Left * Dist;
Vector3D Up = MainController.WorldMatrix.Up * Dist;
Vector3D Down = MainController.WorldMatrix.Down * Dist;
Echo(Dist.ToString());
// Declare and blank out strings
string Package = "";
string DirStr = "";
// List
List <Vector3D> AssignedDrones = new List <Vector3D>(); //Actual list of assigned vector points which represent each drone.
// Storage & Run checks
if (Command == "")
{
Command = DefaultCommand;
}
if (Form == "")
{
Form = DefaultForm;
}
// Argument checks
if (Argument == "Reset")
{
Storage = "";
DirStr = "";
Package = "";
Command = "";
Form = "";
AssignedDrones.Clear();
//Subordiantes.Clear();
}
if (Argument.Contains("C.")) //Set command or formation
{
Command = Argument;
}
else if (Argument.Contains("F."))
{
Form = Argument;
}
//Formations
if (Form != "")
{
bool DirMem = false;//Thanks Digi!
if (Form == "F.Line")
{
int j = 1;
for (int i = 1; i < DroneCount; i++) //Position adding
{
i = (DirMem ? (i - j) : i); //Every other
AssignedDrones.Add(((DirMem ? Left : Right) * i) + GblPosition);
DirMem = !DirMem;
}
}
if (Form == "F.Wedge")
{
int j = 1;
for (int i = 1; i < DroneCount; i++) //Position adding
{
i = (DirMem ? (i - j) : i); //Every other
AssignedDrones.Add(((Back + (DirMem ? (Left) : Right)) * i) + GblPosition);
DirMem = !DirMem;
}
}
if (Form == "F.Inverted Wedge")
{
int j = 1;
for (int i = 1; i < DroneCount; i++) //Position adding
{
i = (DirMem ? (i - j) : i); //Every other
AssignedDrones.Add(((Forward + (DirMem ? (Left) : Right)) * i) + GblPosition);
DirMem = !DirMem;
}
}
if (Form == "F.Right Echelon")
{
for (int i = 1; i < DroneCount; i++)//Position adding
{
AssignedDrones.Add((Back + Right) * i + GblPosition);
}
}
if (Form == "F.Left Echelon")
{
for (int i = 1; i < DroneCount; i++)//Position adding
{
AssignedDrones.Add((Back + Left) * i + GblPosition);
}
}
if (Form == "F.Dual Column")
{
//AssignedDrones.Add(Right + Forward + GblPosition); AssignedDrones.Add(Left + Forward + GblPosition);
//AssignedDrones.Add(Right + 2 * Forward + GblPosition); AssignedDrones.Add(Left + 2 * Forward + GblPosition);
//AssignedDrones.Add(Right + 3 * Forward + GblPosition); AssignedDrones.Add(Left + 3 * Forward + GblPosition);
//AssignedDrones.Add(Right + 4 * Forward + GblPosition); AssignedDrones.Add(Left + 4 * Forward + GblPosition);
//AssignedDrones.Add(Right + 5 * Forward + GblPosition); AssignedDrones.Add(Left + 5 * Forward + GblPosition);
//AssignedDrones.Add(Right + 6 * Forward + GblPosition); AssignedDrones.Add(Left + 6 * Forward + GblPosition);
for (int i = 1; i < DroneCount / 2 + 1; i++)
{
AssignedDrones.Add(Right + Forward * i + GblPosition);
AssignedDrones.Add(Left + Forward * i + GblPosition);
}
}
if (Form == "F.Box")
{
AssignedDrones.Add(Up + Right + GblPosition);
AssignedDrones.Add(Up + Left + GblPosition);
AssignedDrones.Add(2 * Right + GblPosition);
AssignedDrones.Add(2 * Left + GblPosition);
AssignedDrones.Add(Down + Right + GblPosition);
AssignedDrones.Add(Down + Left + GblPosition);
AssignedDrones.Add(2 * Up + 2 * Right + GblPosition);
AssignedDrones.Add(2 * Up + 2 * Left + GblPosition);
AssignedDrones.Add(3 * Right + GblPosition);
AssignedDrones.Add(3 * Left + GblPosition);
AssignedDrones.Add(2 * Down + 2 * Right + GblPosition);
AssignedDrones.Add(2 * Down + 2 * Left + GblPosition);
for (int i = 12; i < DroneCount; i++)//Position adding
{
AssignedDrones.Add(Back * i + GblPosition);
}//Extras: Single Column
}
if (Form == "F.DoubleBox")
{
AssignedDrones.Add(4 * Forward + Up + Right + GblPosition);
AssignedDrones.Add(4 * Forward + Up + Left + GblPosition);
AssignedDrones.Add(4 * Forward + 2 * Right + GblPosition);
AssignedDrones.Add(4 * Forward + 2 * Left + GblPosition);
AssignedDrones.Add(4 * Forward + Down + Right + GblPosition);
AssignedDrones.Add(4 * Forward + Down + Left + GblPosition);
AssignedDrones.Add(2 * Forward + 2 * Up + 2 * Right + GblPosition);
AssignedDrones.Add(2 * Forward + 2 * Up + 2 * Left + GblPosition);
AssignedDrones.Add(2 * Forward + 3 * Right + GblPosition);
AssignedDrones.Add(2 * Forward + 3 * Left + GblPosition);
AssignedDrones.Add(2 * Forward + 2 * Down + 2 * Right + GblPosition);
AssignedDrones.Add(2 * Forward + 2 * Down + 2 * Left + GblPosition);
for (int i = 12; i < DroneCount; i++)//Position adding
{
AssignedDrones.Add(Back * i + GblPosition);
}//Extras: Single Column
}
if (Form == "F.Vangard1")
{
AssignedDrones.Add(10 * Forward + Up + Right + GblPosition);
AssignedDrones.Add(10 * Forward + Up + Left + GblPosition);
AssignedDrones.Add(10 * Forward + 2 * Right + GblPosition);
AssignedDrones.Add(10 * Forward + 2 * Left + GblPosition);
AssignedDrones.Add(10 * Forward + Down + Right + GblPosition);
AssignedDrones.Add(10 * Forward + Down + Left + GblPosition);
AssignedDrones.Add(2 * Forward + 2 * Up + 2 * Right + GblPosition);
AssignedDrones.Add(2 * Forward + 2 * Up + 2 * Left + GblPosition);
AssignedDrones.Add(2 * Forward + 3 * Right + GblPosition);
AssignedDrones.Add(2 * Forward + 3 * Left + GblPosition);
AssignedDrones.Add(2 * Forward + 2 * Down + 2 * Right + GblPosition);
AssignedDrones.Add(2 * Forward + 2 * Down + 2 * Left + GblPosition);
for (int i = 12; i < DroneCount; i++)//Position adding
{
AssignedDrones.Add(Back * i + GblPosition);
}//Extras: Single Column
}
if (Form == "F.Vangard2")
{
AssignedDrones.Add(2 * Forward + Up + Right + GblPosition);
AssignedDrones.Add(2 * Forward + Up + Left + GblPosition);
AssignedDrones.Add(2 * Forward + 2 * Right + GblPosition);
AssignedDrones.Add(2 * Forward + 2 * Left + GblPosition);
AssignedDrones.Add(2 * Forward + Down + Right + GblPosition);
AssignedDrones.Add(2 * Forward + Down + Left + GblPosition);
AssignedDrones.Add(10 * Forward + 2 * Up + 2 * Right + GblPosition);
AssignedDrones.Add(10 * Forward + 2 * Up + 2 * Left + GblPosition);
AssignedDrones.Add(10 * Forward + 3 * Right + GblPosition);
AssignedDrones.Add(10 * Forward + 3 * Left + GblPosition);
AssignedDrones.Add(10 * Forward + 2 * Down + 2 * Right + GblPosition);
AssignedDrones.Add(10 * Forward + 2 * Down + 2 * Left + GblPosition);
for (int i = 12; i < DroneCount; i++)//Position adding
{
AssignedDrones.Add(Back * i + GblPosition);
}//Extras: Single Column
}
if (Form == "F.VangardAll")
{
AssignedDrones.Add(10 * Forward + Up + Right + GblPosition);
AssignedDrones.Add(10 * Forward + Up + Left + GblPosition);
AssignedDrones.Add(10 * Forward + 2 * Right + GblPosition);
AssignedDrones.Add(10 * Forward + 2 * Left + GblPosition);
AssignedDrones.Add(10 * Forward + Down + Right + GblPosition);
AssignedDrones.Add(10 * Forward + Down + Left + GblPosition);
AssignedDrones.Add(10 * Forward + 2 * Up + 2 * Right + GblPosition);
AssignedDrones.Add(10 * Forward + 2 * Up + 2 * Left + GblPosition);
AssignedDrones.Add(10 * Forward + 3 * Right + GblPosition);
AssignedDrones.Add(10 * Forward + 3 * Left + GblPosition);
AssignedDrones.Add(10 * Forward + 2 * Down + 2 * Right + GblPosition);
AssignedDrones.Add(10 * Forward + 2 * Down + 2 * Left + GblPosition);
for (int i = 12; i < DroneCount; i++)//Position adding
{
AssignedDrones.Add(Back * i + GblPosition);
}//Extras: Single Column
}
if (Form == "F.Dense")
{
AssignedDrones.Add(1.0 * Forward + 0.5 * Up + 0.5 * Right + GblPosition);
AssignedDrones.Add(1.0 * Forward + 0.5 * Up + 0.5 * Left + GblPosition);
AssignedDrones.Add(1.0 * Forward + 1.0 * Right + GblPosition);
AssignedDrones.Add(1.0 * Forward + 1.0 * Left + GblPosition);
AssignedDrones.Add(1.0 * Forward + 0.5 * Down + 0.5 * Right + GblPosition);
AssignedDrones.Add(1.0 * Forward + 0.5 * Down + 0.5 * Left + GblPosition);
AssignedDrones.Add(1.0 * Up + 1.0 * Right + GblPosition);
AssignedDrones.Add(1.0 * Up + 1.0 * Left + GblPosition);
AssignedDrones.Add(1.5 * Right + GblPosition);
AssignedDrones.Add(1.5 * Left + GblPosition);
AssignedDrones.Add(1.0 * Down + 1.0 * Right + GblPosition);
AssignedDrones.Add(1.0 * Down + 1.0 * Left + GblPosition);
for (int i = 12; i < DroneCount; i++)//Position adding
{
AssignedDrones.Add(Back * i + GblPosition);
}//Extras: Single Column
}
if (Form == "F.Diffuse")
{
AssignedDrones.Add(4 * Forward + 2 * Up + 2 * Right + GblPosition);
AssignedDrones.Add(4 * Forward + 2 * Up + 2 * Left + GblPosition);
AssignedDrones.Add(4 * Forward + 4 * Right + GblPosition);
AssignedDrones.Add(4 * Forward + 4 * Left + GblPosition);
AssignedDrones.Add(4 * Forward + 2 * Down + 2 * Right + GblPosition);
AssignedDrones.Add(4 * Forward + 2 * Down + 2 * Left + GblPosition);
AssignedDrones.Add(2 * Forward + 4 * Up + 4 * Right + GblPosition);
AssignedDrones.Add(2 * Forward + 4 * Up + 4 * Left + GblPosition);
AssignedDrones.Add(2 * Forward + 6 * Right + GblPosition);
AssignedDrones.Add(2 * Forward + 6 * Left + GblPosition);
AssignedDrones.Add(2 * Forward + 4 * Down + 4 * Right + GblPosition);
AssignedDrones.Add(2 * Forward + 4 * Down + 4 * Left + GblPosition);
for (int i = 12; i < DroneCount; i++)//Position adding
{
AssignedDrones.Add(Back * i + GblPosition);
}//Extras: Single Column
}
if (Form == "F.Sphere")
{
AssignedDrones.Add(2 * Forward + 1 * Up + 1 * Right + GblPosition);
AssignedDrones.Add(2 * Forward + 1 * Up + 1 * Left + GblPosition);
AssignedDrones.Add(2 * Forward + 1 * Down + 1 * Right + GblPosition);
AssignedDrones.Add(2 * Forward + 1 * Down + 1 * Left + GblPosition);
AssignedDrones.Add(2 * Up + GblPosition);
AssignedDrones.Add(2 * Right + GblPosition);
AssignedDrones.Add(2 * Left + GblPosition);
AssignedDrones.Add(2 * Down + GblPosition);
AssignedDrones.Add(2 * Back + 1 * Up + 1 * Right + GblPosition);
AssignedDrones.Add(2 * Back + 1 * Up + 1 * Left + GblPosition);
AssignedDrones.Add(2 * Back + 1 * Down + 1 * Right + GblPosition);
AssignedDrones.Add(2 * Back + 1 * Down + 1 * Left + GblPosition);
for (int i = 12; i < DroneCount; i++)//Position adding
{
AssignedDrones.Add(Back * i + GblPosition);
}//Extras: Single Column
}
if (Form == "F.Single Column")
{
for (int i = 1; i < DroneCount; i++)//Position adding
{
AssignedDrones.Add(Back * i + GblPosition);
}
}
//Commands
if (Command == "C.Land")
{
double Height;
bool InPlanet = MainController.TryGetPlanetElevation(MyPlanetElevation.Surface, out Height);
if (InPlanet)
{
for (int i = 1; i < AssignedDrones.Count; i++)
{
AssignedDrones[i] = AssignedDrones[i] + ((Down / Dist) * Height);
}
}
else
{
Echo("Not in planet gravity: Subordinates commanded to wait.");
Command = ("C.Wait");
}
}
}
DroneCount--; // Remove 1 from drone count to reset to 0 index modification
//Compile and transmit
if (AssignedDrones.Count == 0)
{
Echo("Error: No drones assigned. Either internal error or argument has not been stated.");
}
else
{
DirStr = (GblPosition.X.ToString() + "," + GblPosition.Y.ToString() + "," + GblPosition.Z.ToString());
Package = Channel + ":" + DirStr + ":" + Command + ":" + Form + ":";
// TODO 指揮船の向きを追加
// Create Package
foreach (Vector3D drone in AssignedDrones)
{
Package += (Math.Round(drone.X).ToString() + "," + Math.Round(drone.Y).ToString() + "," + Math.Round(drone.Z).ToString()) + ":";
}
//TODO LastPosition never assigned
if (Distance(GblPosition, LastPosition) < ControlFactor)
{
Echo("Idle Mode");
}
else
{
// Broadcast Package
bool Transmitted = Antennae[0].TransmitMessage(Package, MyTransmitTarget.Default);
if (Transmitted)
{
Echo("Broadcasting Secure Package. Channel: " + Channel);
}
else
{
Echo("Error: Transmit Failed");
}
Echo("DroneCount: " + DroneCount);
Echo("Formation: " + Form);
Echo("Command: " + Command);
AssignedDrones.Clear();
}
}
}
public void Drive(Vector3D Destination)
{
if (Destination == Vector3D.Zero)
{
return;
}
if (GravAngle > Math.PI * 0.1)
{
foreach (var engine in HoverEngines)
{
engine.Block.Enabled = true;
engine.OverridePercentage = 0;
engine.AltitudeMin = DesiredAltitude / VertiHoriForceRatio;
}
return;
}
var gravDir = Controller.GetNaturalGravity();
var gravStr = gravDir.Length();
gravDir.Normalize();
var destDir = Destination - Controller.WorldMatrix.Translation;
destDir -= VectorHelpers.VectorProjection(destDir, gravDir);
var dist = destDir.Length();
Arrived = dist < 20;
destDir.Normalize();
var maxSpeed = SpeedLimit == 0 ? Math.Min(100, GetMaxSpeedFromBrakingDistance((float)dist)) : SpeedLimit;
var currentVel = Controller.GetShipVelocities().LinearVelocity;
var horiVel = currentVel - VectorHelpers.VectorProjection(Controller.GetShipVelocities().LinearVelocity, gravDir);
var vertiVel = currentVel.Dot(-gravDir);
var accelDir = maxSpeed * destDir - horiVel;
accelDir.Normalize();
var mass = Controller.CalculateShipMass().PhysicalMass;
var gravForce = gravStr * mass;
DirForces.Clear();
Dirs.Clear();
Dirs.Add(Controller.WorldMatrix.Forward);
Dirs.Add(Controller.WorldMatrix.Down);
Dirs.Add(Controller.WorldMatrix.Left);
float alpha = 0;
double altitude;
Controller.TryGetPlanetElevation(MyPlanetElevation.Surface, out altitude);
DesiredVerticalVel = DesiredAltitude - altitude;
var altAdjust = DesiredVerticalVel - vertiVel;
Vector3D PrimaryDriveDir = Vector3D.Zero;
Vector3D SecondaryDriveDir = Vector3D.Zero;
Vector3D OpposDriveDir = Vector3D.Zero;
foreach (var dir in Dirs)
{
var engineDir = dir;
engineDir -= VectorHelpers.VectorProjection(engineDir, gravDir);
engineDir.Normalize();
var engineAngleDiff = VectorHelpers.VectorAngleBetween(engineDir, accelDir);
if (engineAngleDiff < 0.3333333333333 * Math.PI)
{
DirForces[dir] = 0;
OpposDriveDir = dir;
}
else if (engineAngleDiff > 2 * 0.3333333333333 * Math.PI)
{
DirForces[dir] = 1f;
PrimaryDriveDir = dir;
}
else
{
var aPrime = 1 / (TrigHelpers.fastTan(engineAngleDiff - Math.PI * 0.333333333) * sqrt3inv + 1);
DirForces[dir] = (float)(1 - aPrime) * 2;
SecondaryDriveDir = dir;
alpha = DirForces[dir];
}
}
var primaryDriveForce = MaxForce * VerticalForceRatio;
var secondaryDriveForce = primaryDriveForce * alpha;
var totalUpForce = primaryDriveForce + secondaryDriveForce;
var multiplier = (gravForce + altAdjust * mass) / totalUpForce;
Status = $"{altAdjust}, {dist}";
if (PrimaryDriveDir != Vector3D.Zero)
{
DirForces[PrimaryDriveDir] = Math.Max(0.01f, (float)multiplier);
}
if (SecondaryDriveDir != Vector3D.Zero)
{
DirForces[SecondaryDriveDir] = (float)Math.Max(0.01f, alpha * (float)multiplier);
}
var altMin = 1;
if (altitude > 7)
{
altMin = 6;
}
if (altitude > 40)
{
altMin = 10;
}
foreach (var engine in HoverEngines)
{
engine.AltitudeMin = altMin;
engine.Block.Enabled = OpposDriveDir != engine.Block.WorldMatrix.Forward;
engine.OverridePercentage = DirForces[engine.Block.WorldMatrix.Forward];
}
}
private void StabilizePod()
{
//---Get speed
double currentSpeed = referenceBlock.GetShipSpeed();
Vector3D vectDistanceToEnd = destination.ToVector3D() - referenceBlock.GetPosition();
Vector3D shipVelocityVec = referenceBlock.GetShipVelocities().LinearVelocity;
double brakingSpeed = VectorUtils.Projection(shipVelocityVec, vectDistanceToEnd).Length() * Math.Sign(shipVelocityVec.Dot(vectDistanceToEnd));
//---Determine if we should manually override brake controls
double distanceToEnd = vectDistanceToEnd.Length();
//--- Manage gravity
Vector3D gravityVec = referenceBlock.GetNaturalGravity();
double gravityVecMagnitude = gravityVec.Length();
if (gravityVecMagnitude > 0.0001)
{
double distanceToSurface;
referenceBlock.TryGetPlanetElevation(MyPlanetElevation.Surface, out distanceToSurface);
// TODO take into account this distance if we fly near the surface
}
distanceToEnd -= shipCenterToEdge;
double distanceToStartBraking = GetBrakingDistanceThreshold(gravityVecMagnitude, shipVelocityVec);
//this gives us a good safety cushion for stabilization procedures
double distanceToStartStabilize = distanceToStartBraking + 10 * currentSpeed;
bool shouldBrake = false;
bool shouldStabilize = false;
if (distanceToEnd < 100 && currentSpeed < 1)
{
timeSpentStationary += timeCurrentCycle;
}
else
{
timeSpentStationary = 0;
shouldStabilize = distanceToEnd <= distanceToStartStabilize;
shouldBrake = distanceToEnd <= distanceToStartBraking;
//kills dampeners to stop their interference with landing procedures
if (shouldBrake)
{
referenceBlock.DampenersOverride = false;
}
}
if (shouldBrake)
{
bool isBraking = brakingSpeed > FROM_BRAKING_TO_END_SPEED;
if (isBraking)
{
BrakingOn();
}
else
{
if (attemptToLand)
{
BrakingThrust(brakingSpeed, gravityVecMagnitude);
}
else
{
StopSystem(); // Fin
}
}
}
else
{
BrakingOff();
}
if (shouldStabilize)
{
//---Get Roll and Pitch Angles
Vector3D alignmentVec = brakingSpeed > FROM_BRAKING_TO_END_SPEED || gravityVecMagnitude <= 0 ? shipVelocityVec : gravityVec;
//---Dir'n vectors of the reference block
var referenceMatrix = referenceBlock.WorldMatrix;
var referenceForward = referenceMatrix.Forward;
var referenceLeft = referenceMatrix.Left;
var referenceUp = referenceMatrix.Up;
var referenceOrigin = referenceMatrix.Translation;
// Roll = rotation around forward/backward axis => to roll
// Pitch = rotation around left/right axis => to go up or down
// Yaw = rotation around up/down axis => to go left or right
double forAngleRadPitch = MathHelper.Clamp(alignmentVec.Dot(referenceForward) / alignmentVec.Length(), -1, 1);
double anglePitch = Math.Acos(forAngleRadPitch); // TODO test mais on freine vers l'avant pas le bas : - Math.PI / 2;
// ???
Vector3D relativeLeftVec = referenceForward.Cross(alignmentVec);
double forAngleRadRoll = MathHelper.Clamp(referenceLeft.Dot(relativeLeftVec) / relativeLeftVec.Length(), -1, 1);
double angleRoll = Math.Acos(forAngleRadRoll);
angleRoll *= Math.Sign(VectorUtils.Projection(referenceLeft, alignmentVec).Dot(alignmentVec)); //ccw is positive
anglePitch *= -1;
angleRoll *= -1;
double roll_deg = Math.Round(angleRoll / Math.PI * 180);
double pitch_deg = Math.Round(anglePitch / Math.PI * 180);
//---Angle controller
double rollSpeed = Math.Round(angleRoll, 2);
double pitchSpeed = Math.Round(anglePitch, 2);
//---Enforce rotation speed limit
if (Math.Abs(rollSpeed) + Math.Abs(pitchSpeed) > 2 * Math.PI)
{
double scale = 2 * Math.PI / (Math.Abs(rollSpeed) + Math.Abs(pitchSpeed));
rollSpeed *= scale;
pitchSpeed *= scale;
}
ApplyGyroOverride(pitchSpeed, 0, -rollSpeed);
}
else
{
foreach (IMyGyro thisGyro in gyros)
{
thisGyro.GyroOverride = false;
}
}
}
private void UpdateThrusterPower()
{
MotionDebug = "";
powerPosX = 0;
powerNegX = 0;
powerPosY = 0;
powerNegY = 0;
powerPosZ = 0;
powerNegZ = 0;
bool inAtmosphere = false;
double airDensity = 0;
double altitude;
if (shipController.TryGetPlanetElevation(MyPlanetElevation.Sealevel, out altitude))
{
inAtmosphere = altitude < atmosphereAltitude;
if (inAtmosphere)
{
airDensity = getAirDensityAtPlanet(altitude);
}
}
foreach (IMyThrust thruster in thrusters)
{
Base6Directions.Direction direction = Base6Directions.GetFlippedDirection(thruster.Orientation.Forward);
ThrusterInfo info = thrusterTypes.GetValueOrDefault(thruster.BlockDefinition.SubtypeId);
if (info == null)
{
program.Echo("Unknown thruster type: " + thruster.BlockDefinition.SubtypeId);
continue;
}
double power = info.GetPower(inAtmosphere, airDensity);
switch (direction)
{
case Base6Directions.Direction.Right:
powerPosX += power;
break;
case Base6Directions.Direction.Left:
powerNegX += power;
break;
case Base6Directions.Direction.Up:
powerPosY += power;
break;
case Base6Directions.Direction.Down:
powerNegY += power;
break;
case Base6Directions.Direction.Backward:
powerPosZ += power;
break;
case Base6Directions.Direction.Forward:
powerNegZ += power;
break;
}
}
}
void doModeGoTarget()
{
int iMode = thisProgram.wicoControl.IMode;
int iState = thisProgram.wicoControl.IState;
// StatusLog("clear", textPanelReport);
// StatusLog(moduleName + ":Going Target!", textPanelReport);
// StatusLog(moduleName + ":GT: iState=" + iState.ToString(), textPanelReport);
// bWantFast = true;
thisProgram.Echo("Going Target: state=" + iState.ToString());
if (NAVTargetName != "")
{
thisProgram.Echo(NAVTargetName);
}
string sNavDebug = "";
sNavDebug += "GT:S=" + iState;
// sNavDebug += " MinE=" + NAVGravityMinElevation;
// ResetMotion();
IMyShipController shipController = thisProgram.wicoBlockMaster.GetMainController();
Vector3D vNG = shipController.GetNaturalGravity();
double dGravity = vNG.Length();
if (iState == 0)
{
thisProgram.wicoTravelMovement.ResetTravelMovement();
// sStartupError+="\nStart movemenet: ArrivalMode="+NAVArrivalMode+" State="+NAVArrivalState;
// if ((craft_operation & CRAFT_MODE_SLED) > 0)
if (thisProgram.wicoWheels.HasSledWheels())
{
bSled = true;
if (shipSpeedMax > 45)
{
shipSpeedMax = 45;
}
}
else
{
bSled = false;
}
// if ((craft_operation & CRAFT_MODE_ROTOR) > 0)
if (thisProgram.wicoNavRotors.NavRotorCount() > 0)
{
bRotor = true;
if (shipSpeedMax > 15)
{
shipSpeedMax = 15;
}
}
else
{
bRotor = false;
}
// if ((craft_operation & CRAFT_MODE_WHEEL) > 0)
if (thisProgram.wicoWheels.HasWheels())
{
bWheels = true;
// if (shipSpeedMax > 15) shipSpeedMax = 15;
}
else
{
bWheels = false;
}
// GyroControl.SetRefBlock(shipOrientationBlock);
// TODO: Put a timer on this so it's not done Update1
double elevation = 0;
shipController.TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation);
if (!bSled && !bRotor)
{ // if flying ship
// make sure set to default
if (thisProgram.wicoBlockMaster.DesiredMinTravelElevation < 0)
{
thisProgram.wicoBlockMaster.DesiredMinTravelElevation = 75; // for EFM getting to target 'arrived' radius
}
}
if (bValidNavTarget)
{
if (elevation > thisProgram.wicoBlockMaster.HeightInMeters())
{
iState = 150;
}
else
{
iState = 160;
}
}
else
{
thisProgram.wicoControl.SetMode(WicoControl.MODE_ATTENTION); //else setMode(MODE_ATTENTION);
}
thisProgram.wicoControl.WantFast(); // bWantFast = true;
}
else if (iState == 150)
{
thisProgram.wicoControl.WantFast();// bWantFast = true;
if (dGravity > 0)
{
double elevation = 0;
shipController.TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation);
sNavDebug += " E=" + elevation.ToString("0.0");
float fSaveAngle = thisProgram.wicoGyros.GetMinAngle(); // minAngleRad;
thisProgram.wicoGyros.SetMinAngle(0.1f); // minAngleRad = 0.1f;
bool bAligned = GyroMain("", vNG, shipController);
sNavDebug += " Aligned=" + bAligned.ToString();
thisProgram.Echo("bAligned=" + bAligned.ToString());
thisProgram.wicoGyros.SetMinAngle(fSaveAngle); //minAngleRad = fSaveAngle;
if (bAligned || elevation < thisProgram.wicoBlockMaster.HeightInMeters() * 2)
{
thisProgram.wicoGyros.gyrosOff();
if (thisProgram.wicoBlockMaster.DesiredMinTravelElevation > 0)
{
iState = 155;
}
else
{
iState = 160;
}
}
}
else
{
iState = 160;
}
}
else if (iState == 151)
{
thisProgram.wicoControl.WantFast();// bWantFast = true;
if (dGravity > 0 || btmWheels)
{
double elevation = 0;
shipController.TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation);
sNavDebug += " E=" + elevation.ToString("0.0");
float fSaveAngle = thisProgram.wicoGyros.GetMinAngle(); // minAngleRad;
thisProgram.wicoGyros.SetMinAngle(0.1f); // minAngleRad = 0.1f;
bool bAligned = GyroMain("", vNG, shipOrientationBlock);
sNavDebug += " Aligned=" + bAligned.ToString();
thisProgram.Echo("bAligned=" + bAligned.ToString());
thisProgram.wicoGyros.SetMinAngle(fSaveAngle); //minAngleRad = fSaveAngle;
if (bAligned || elevation < thisProgram.wicoBlockMaster.HeightInMeters() * 2)
{
thisProgram.wicoGyros.gyrosOff();
if (thisProgram.wicoBlockMaster.DesiredMinTravelElevation > 0)
{
iState = 155;
}
else
{
iState = 160;
}
}
else
{
iState = 150; // try again to be aligned.
}
}
else
{
iState = 160;
}
}
else if (iState == 155)
{ // for use in gravity: aim at location using yaw only
thisProgram.wicoControl.WantFast(); // bWantFast = true;
if (bWheels)
{
iState = 160;
return;
}
if (dGravity > 0)
{
bool bAligned = GyroMain("", vNG, shipController);
sNavDebug += " Aligned=" + bAligned.ToString();
double yawangle = -999;
yawangle = thisProgram.CalculateYaw(vNavTarget, shipController);
bool bAimed = Math.Abs(yawangle) < 0.1; // NOTE: 2x allowance
thisProgram.Echo("yawangle=" + yawangle.ToString());
sNavDebug += " Yaw=" + yawangle.ToString("0.00");
if (!bAimed)
{
if (btmRotor)
{
thisProgram.Echo("Rotor");
thisProgram.wicoNavRotors.DoRotorRotate(yawangle);
}
else // use for both sled and flight
{
thisProgram.wicoGyros.DoRotate(yawangle, "Yaw");
}
}
if (bAligned && bAimed)
{
thisProgram.wicoGyros.gyrosOff();
iState = 160;
}
else if (bAligned && Math.Abs(yawangle) < 0.5)
{
float atmo;
float hydro;
float ion;
thisProgram.wicoThrusters.CalculateHoverThrust(shipController, thrustForwardList, out atmo, out hydro, out ion);
atmo += 1;
hydro += 1;
ion += 1;
thisProgram.wicoThrusters.powerUpThrusters(thrustForwardList, atmo, WicoThrusters.thrustatmo);
thisProgram.wicoThrusters.powerUpThrusters(thrustForwardList, hydro, WicoThrusters.thrusthydro);
thisProgram.wicoThrusters.powerUpThrusters(thrustForwardList, ion, WicoThrusters.thrustion);
}
else
{
thisProgram.wicoThrusters.powerDownThrusters(thrustForwardList);
}
}
else
{
iState = 160;
}
}
else if (iState == 156)
{
// realign gravity
thisProgram.wicoControl.WantFast();// bWantFast = true;
bool bAimed = GyroMain("", grav, shipOrientationBlock);
if (bAimed)
{
thisProgram.wicoGyros.gyrosOff();
iState = 160;
}
}
else if (iState == 160)
{ // 160 move to Target
thisProgram.Echo("Moving to Target");
Vector3D vTargetLocation = vNavTarget;
double velocityShip = shipController.GetShipSpeed();
Vector3D vVec = vTargetLocation - shipController.GetPosition();
double distance = vVec.Length();
thisProgram.Echo("distance=" + niceDoubleMeters(distance));
thisProgram.Echo("velocity=" + velocityShip.ToString("0.00"));
// StatusLog("clear", sledReport);
string sTarget = "Moving to Target";
if (NAVTargetName != "")
{
sTarget = "Moving to " + NAVTargetName;
}
// StatusLog(sTarget + "\nD:" + niceDoubleMeters(distance) + " V:" + velocityShip.ToString(velocityFormat), sledReport);
// StatusLog(sTarget + "\nDistance: " + niceDoubleMeters(distance) + "\nVelocity: " + niceDoubleMeters(velocityShip) + "/s", textPanelReport);
if (bGoOption && (distance < arrivalDistanceMin))
{
iState = 500;
thisProgram.Echo("we have arrived");
thisProgram.wicoControl.WantFast();// bWantFast = true;
return;
}
// debugGPSOutput("TargetLocation", vTargetLocation);
bool bDoTravel = true;
if (thisProgram.wicoBlockMaster.DesiredMinTravelElevation > 0 && dGravity > 0)
{
double elevation = 0;
MyShipVelocities mysSV = shipController.GetShipVelocities();
Vector3D lv = mysSV.LinearVelocity;
// ASSUMES: -up = gravity down Assuming ship orientation
var upVec = shipController.WorldMatrix.Up;
var vertVel = Vector3D.Dot(lv, upVec);
// thisProgram.Echo("LV=" + Vector3DToString(lv));
// sNavDebug += " LV=" + Vector3DToString(lv);
// sNavDebug += " vertVel=" + vertVel.ToString("0.0");
// sNavDebug += " Hvel=" + lv.Y.ToString("0.0");
// NOTE: Elevation is only updated by game every 30? ticks. so it can be WAY out of date based on movement
shipController.TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation);
sNavDebug += " E=" + elevation.ToString("0.0");
sNavDebug += " V=" + velocityShip.ToString("0.00");
thisProgram.Echo("Elevation=" + elevation.ToString("0.0"));
thisProgram.Echo("MinEle=" + thisProgram.wicoBlockMaster.DesiredMinTravelElevation.ToString("0.0"));
// double stopD = calculateStoppingDistance(thrustUpList, velocityShip, dGravity);
double stopD = 0;
if (vertVel < 0)
{
stopD = thisProgram.wicoThrusters.calculateStoppingDistance(thrustUpList, Math.Abs(vertVel), dGravity);
}
double maxStopD = thisProgram.wicoThrusters.calculateStoppingDistance(thrustUpList, thisProgram.wicoControl.fMaxWorldMps, dGravity);
float atmo;
float hydro;
float ion;
thisProgram.wicoThrusters.CalculateHoverThrust(thrustUpList, out atmo, out hydro, out ion);
// sNavDebug += " SD=" + stopD.ToString("0");
if (
// !bSled && !bRotor &&
thisProgram.wicoBlockMaster.DesiredMinTravelElevation > 0)
{
if (
vertVel < -0.5 && // we are going downwards
(elevation - stopD * 2) < thisProgram.wicoBlockMaster.DesiredMinTravelElevation)
{ // too low. go higher
// Emergency thrust
sNavDebug += " EM UP!";
bool bAligned = GyroMain("", grav, shipOrientationBlock);
thisProgram.wicoThrusters.powerUpThrusters(thrustUpList, 100);
bDoTravel = false;
thisProgram.wicoControl.WantFast();// bWantFast = true;
}
else if (elevation < thisProgram.wicoBlockMaster.DesiredMinTravelElevation)
{
// push upwards
atmo += Math.Min(5f, (float)shipSpeedMax);
hydro += Math.Min(5f, (float)shipSpeedMax);
ion += Math.Min(5f, (float)shipSpeedMax);
sNavDebug += " UP! A" + atmo.ToString("0.00"); // + " H"+hydro.ToString("0.00") + " I"+ion.ToString("0.00");
//powerUpThrusters(thrustUpList, 100);
thisProgram.wicoThrusters.powerUpThrusters(thrustUpList, atmo, thrustatmo);
thisProgram.wicoThrusters.powerUpThrusters(thrustUpList, hydro, thrusthydro);
thisProgram.wicoThrusters.powerUpThrusters(thrustUpList, ion, thrustion);
}
else if (elevation > (maxStopD + thisProgram.wicoBlockMaster.DesiredMinTravelElevation * 1.25))
{
// if we are higher than maximum possible stopping distance, go down fast.
sNavDebug += " SUPERHIGH";
// Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity();
// bool bAligned = GyroMain("", grav, shipOrientationBlock);
thisProgram.wicoThrusters.powerDownThrusters(thrustUpList, thrustAll, true);
bool bAligned = GyroMain("", grav, shipOrientationBlock);
if (!bAligned)
{
thisProgram.wicoControl.WantFast();// bWantFast = true;
bDoTravel = false;
}
// powerUpThrusters(thrustUpList, 1f);
}
else if (
elevation > thisProgram.wicoBlockMaster.DesiredMinTravelElevation * 2 // too high
// && ((elevation-stopD)>NAVGravityMinElevation) // we can stop in time.
// && velocityShip < shipSpeedMax * 1.1 // to fast in any direction
// && Math.Abs(lv.X) < Math.Min(25, shipSpeedMax) // not too fast
// && Math.Abs(lv.Y) < Math.Min(25, shipSpeedMax) // not too fast downwards (or upwards)
)
{ // too high
sNavDebug += " HIGH";
//DOWN! A" + atmo.ToString("0.00");// + " H" + hydro.ToString("0.00") + " I" + ion.ToString("0.00");
if (vertVel > 2) // going up
{ // turn off thrusters.
sNavDebug += " ^";
thisProgram.wicoThrusters.powerDownThrusters(thrustUpList, thrustAll, true);
}
else if (vertVel < -0.5) // going down
{
sNavDebug += " v";
if (vertVel > (-Math.Min(15, shipSpeedMax)))
{
// currently descending at less than desired
atmo -= Math.Max(25f, Math.Min(5f, (float)velocityShip / 2));
hydro -= Math.Max(25f, Math.Min(5f, (float)velocityShip / 2));
ion -= Math.Max(25f, Math.Min(5f, (float)velocityShip / 2));
sNavDebug += " DOWN! A" + atmo.ToString("0.00"); // + " H" + hydro.ToString("0.00") + " I" + ion.ToString("0.00");
// bDoTravel = false;
}
else
{
// we are descending too fast.
atmo += Math.Max(100f, Math.Min(5f, (float)velocityShip / 2));
hydro += Math.Max(100f, Math.Min(5f, (float)velocityShip / 2));
ion += Math.Max(100f, Math.Min(5f, (float)velocityShip / 2));
sNavDebug += " 2FAST! A" + atmo.ToString("0.00");// + " H" + hydro.ToString("0.00") + " I" + ion.ToString("0.00");
bool bAligned = GyroMain("", grav, shipOrientationBlock);
if (!bAligned)
{
thisProgram.wicoControl.WantFast();// bWantFast = true;
bDoTravel = false;
}
// bDoTravel = false;
}
}
else
{
sNavDebug += " -";
atmo -= 5;
hydro -= 5;
ion -= 5;
}
thisProgram.wicoThrusters.powerUpThrusters(thrustUpList, atmo, thrustatmo);
thisProgram.wicoThrusters.powerUpThrusters(thrustUpList, hydro, thrusthydro);
thisProgram.wicoThrusters.powerUpThrusters(thrustUpList, ion, thrustion);
}
else
{
// normal hover
thisProgram.wicoThrusters.powerDownThrusters(thrustUpList);
}
}
}
if (bDoTravel)
{
thisProgram.Echo("Do Travel");
thisProgram.wicoTravelMovement.doTravelMovement(vTargetLocation, (float)arrivalDistanceMin, 500, 300);
}
else
{
thisProgram.wicoThrusters.powerDownThrusters(thrustForwardList);
}
}
else if (iState == 300)
{ // collision detection
thisProgram.wicoControl.WantFast(); // bWantFast = true;
Vector3D vTargetLocation = vNavTarget;
thisProgram.wicoTravelMovement.ResetTravelMovement();
thisProgram.wicoTravelMovement.calcCollisionAvoid(vTargetLocation, lastDetectedInfo, out vAvoid);
// iState = 301; // testing
iState = 320;
}
else if (iState == 301)
{
// just hold this state
thisProgram.wicoControl.WantFast();// bWantFast = true;
}
else if (iState == 320)
{
// Vector3D vVec = vAvoid - shipOrientationBlock.GetPosition();
// double distanceSQ = vVec.LengthSquared();
thisProgram.Echo("Primary Collision Avoid");
// StatusLog("clear", sledReport);
// StatusLog("Collision Avoid", sledReport);
// StatusLog("Collision Avoid", textPanelReport);
thisProgram.wicoTravelMovement.doTravelMovement(vAvoid, 5.0f, 160, 340);
}
else if (iState == 340)
{ // secondary collision
if (
lastDetectedInfo.Type == MyDetectedEntityType.LargeGrid ||
lastDetectedInfo.Type == MyDetectedEntityType.SmallGrid
)
{
iState = 345;
}
else if (lastDetectedInfo.Type == MyDetectedEntityType.Asteroid
)
{
iState = 350;
}
else
{
iState = 300; // setMode(MODE_ATTENTION);
}
thisProgram.wicoControl.WantFast(); // bWantFast = true;
}
else if (iState == 345)
{
// we hit a grid. align to it
Vector3D[] corners = new Vector3D[BoundingBoxD.CornerCount];
BoundingBoxD bbd = lastDetectedInfo.BoundingBox;
bbd.GetCorners(corners);
GridUpVector = thisProgram.wicoSensors.PlanarNormal(corners[3], corners[4], corners[7]);
GridRightVector = thisProgram.wicoSensors.PlanarNormal(corners[0], corners[1], corners[4]);
thisProgram.wicoControl.WantFast();// bWantFast = true;
iState = 348;
}
else if (iState == 348)
{
thisProgram.wicoControl.WantFast();// bWantFast = true;
if (GyroMain("up", GridUpVector, shipOrientationBlock))
{
iState = 349;
}
}
else if (iState == 349)
{
thisProgram.wicoControl.WantFast();// bWantFast = true;
if (GyroMain("right", GridRightVector, shipOrientationBlock))
{
iState = 350;
}
}
else if (iState == 350)
{
// initEscapeScan(bCollisionWasSensor, !bCollisionWasSensor);
initEscapeScan(bCollisionWasSensor);
thisProgram.wicoTravelMovement.ResetTravelMovement();
dtNavStartShip = DateTime.Now;
iState = 360;
thisProgram.wicoControl.WantFast();// bWantFast = true;
}
else if (iState == 360)
{
// StatusLog("Collision Avoid\nScan for escape route", textPanelReport);
DateTime dtMaxWait = dtNavStartShip.AddSeconds(5.0f);
DateTime dtNow = DateTime.Now;
if (DateTime.Compare(dtNow, dtMaxWait) > 0)
{
thisProgram.wicoControl.SetMode(WicoControl.MODE_ATTENTION);// setMode(MODE_ATTENTION);
// doTriggerMain();
return;
}
if (scanEscape())
{
thisProgram.Echo("ESCAPE!");
iState = 380;
}
thisProgram.wicoControl.WantMedium(); // bWantMedium = true;
// bWantFast = true;
}
else if (iState == 380)
{
// StatusLog("Collision Avoid Travel", textPanelReport);
thisProgram.Echo("Escape Collision Avoid");
thisProgram.wicoTravelMovement.doTravelMovement(vAvoid, 1f, 160, 340);
}
else if (iState == 500)
{ // we have arrived at target
/*
* // check for more nav commands
* if(wicoNavCommands.Count>0)
* {
* wicoNavCommands.RemoveAt(0);
* }
* if(wicoNavCommands.Count>0)
* {
* // another command
* wicoNavCommandProcessNext();
* }
* else
*/
{
// StatusLog("clear", sledReport);
// StatusLog("Arrived at Target", sledReport);
// StatusLog("Arrived at Target", textPanelReport);
sNavDebug += " ARRIVED!";
ResetMotion();
bValidNavTarget = false; // we used this one up.
// float range = RangeToNearestBase() + 100f + (float)velocityShip * 5f;
thisProgram.wicoAntennas.SetMaxPower(false);
thisProgram.wicoSensors.SensorsSleepAll();
// sStartupError += "Finish WP:" + wicoNavCommands.Count.ToString()+":"+NAVArrivalMode.ToString();
// set to desired mode and state
thisProgram.wicoControl.SetMode(NAVArrivalMode);// setMode(NAVArrivalMode);
iState = NAVArrivalState;
// set up defaults for next run (in case they had been changed)
NAVArrivalMode = WicoControl.MODE_ARRIVEDTARGET;
NAVArrivalState = 0;
NAVTargetName = "";
bGoOption = true;
// setMode(MODE_ARRIVEDTARGET);
if (NAVEmulateOld)
{
var tList = GetBlocksContains <IMyTerminalBlock>("NAV:");
for (int i1 = 0; i1 < tList.Count(); i1++)
{
// don't want to get blocks that have "NAV:" in customdata..
if (tList[i1].CustomName.StartsWith("NAV:"))
{
thisProgram.Echo("Found NAV: command:");
tList[i1].CustomName = "NAV: C Arrived Target";
}
}
}
}
thisProgram.wicoControl.WantFast();// bWantFast = true;
// doTriggerMain();
}
// NavDebug(sNavDebug);
}
public void Main(string argument, UpdateType updateSource)
{
if (!ModuleInited())
{
return;
}
if (init_error)
{
Echo(init_message);
ProgBlockLCD.WriteText(init_message);
if (LCD != null)
{
LCD.WriteText(init_message);
}
return;
}
ToLog("", false, false);
Vector3D GravityVector = Cockpit.GetNaturalGravity();
Vector3D GravityNorm = Vector3D.Normalize(GravityVector);
Vector3D VelocityVector = Cockpit.GetShipVelocities().LinearVelocity;
Vector3D VelocityVectorNorm = Vector3D.Normalize(VelocityVector);
Vector3D ForwardVectorNorm = Vector3D.Normalize(Vector3D.Reject(Cockpit.WorldMatrix.Forward, GravityNorm));
Vector3D LeftVectorNorm = Vector3D.Normalize(ForwardVectorNorm.Cross(GravityNorm));
Vector3D ForwardInput = ForwardVectorNorm * Cockpit.MoveIndicator.Z;
Vector3D LeftInput = LeftVectorNorm * Cockpit.MoveIndicator.X;
Vector3D ForwardVelocity = Vector3D.ProjectOnVector(ref VelocityVector, ref ForwardVectorNorm);
Vector3D LeftVelocity = Vector3D.ProjectOnVector(ref VelocityVector, ref LeftVectorNorm);
float UpInput = Cockpit.RollIndicator / 10;
Vector3D AlignVector = Vector3D.Zero;
switch (argument)
{
case "Dampeners":
{
PilotMode = PilotModeEnum.Dampeners;
break;
}
case "Cruise":
{
PilotMode = PilotModeEnum.Cruise;
if ((ForwardVectorNorm + ForwardVelocity).Length() > ForwardVelocity.Length())
{
cruisespeed = (float)ForwardVelocity.Length();
}
else
{
cruisespeed = -(float)ForwardVelocity.Length();
}
break;
}
case "FreeMove":
{
PilotMode = PilotModeEnum.FreeMove;
break;
}
default:
break;
}
if (!Vector3D.IsZero(LeftInput))
{
AlignVector += LeftInput * KI;
}
else
if (PilotMode != PilotModeEnum.FreeMove)
{
AlignVector += LeftVelocity * KV;
}
if (!Vector3D.IsZero(ForwardInput))
{
if (PilotMode == PilotModeEnum.Cruise)
{
cruisespeed += -Cockpit.MoveIndicator.Z / 10;
AlignVector += (ForwardVelocity - ForwardVectorNorm * cruisespeed) * KV;
}
else
{
AlignVector += ForwardInput * KI;
}
}
else
if (PilotMode == PilotModeEnum.Cruise)
{
AlignVector += (ForwardVelocity - ForwardVectorNorm * cruisespeed) * KV;
}
else
if (PilotMode != PilotModeEnum.FreeMove)
{
AlignVector += ForwardVelocity * KV;
}
AlignVector += GravityNorm;
float PitchInput = -(float)AlignVector.Dot(Cockpit.WorldMatrix.Forward);
float RollInput = (float)AlignVector.Dot(Cockpit.WorldMatrix.Left);
float YawInput = Cockpit.RotationIndicator.Y;
foreach (IMyThrust hover in HoverList)
{
altitude = hover.GetValue <float>("altitudemin_slider_L");
altitude += UpInput;
hover.SetValue <float>("altitudemin_slider_L", altitude);
}
foreach (IMyGyro gyro in GyroList)
{
gyro.GyroOverride = true;
gyro.Yaw = YawInput;
gyro.Roll = RollInput;
gyro.Pitch = PitchInput;
}
DisplayUpdateCounter += 1;
if (DisplayUpdateCounter >= DisplayUpdateInterval)
{
ProgBlockLCD.WriteText("\n" + "\n" + "\n" + "Cycle time, µs: " + (runTime * DisplayUpdateFreq).ToString("#0.0") + "\n");
Echo("Cycle time, µs: " + (runTime * DisplayUpdateFreq).ToString("#0.0") + "\n" + init_message);
DisplayUpdateCounter = 0;
runTime = 0d;
ProgBlockLCD.WriteText(PilotModeToString(), true);
string speed = string.Format("Скорость: {0:#0.0} / ", VelocityVector.Length());
if ((ForwardVectorNorm + ForwardVelocity).Length() > ForwardVelocity.Length())
{
speed += string.Format("Вперед: {0:#0.0} / ", ForwardVelocity.Length());
}
else
{
speed += string.Format("Назад: {0:#0.0} / ", ForwardVelocity.Length());
}
if ((LeftVectorNorm + LeftVelocity).Length() > LeftVelocity.Length())
{
speed += string.Format("Влево: {0:#0.0}\n", LeftVelocity.Length());
}
else
{
speed += string.Format("Вправо: {0:#0.0}\n", LeftVelocity.Length());
}
ProgBlockLCD.WriteText(speed, true);
double sla;
double sfa;
Cockpit.TryGetPlanetElevation(MyPlanetElevation.Sealevel, out sla);
Cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface, out sfa);
ProgBlockLCD.WriteText(string.Format("Высота: пар. {0:#0.0} / ур. м. {1:#0.0} / пов. {2:#0.0}\n", altitude, sla, sfa), true);
float thrust = 0;
foreach (IMyThrust hover in HoverList)
{
thrust += hover.MaxThrust;
}
float curmass = Cockpit.CalculateShipMass().PhysicalMass;
float maxmass = thrust / (float)Cockpit.GetNaturalGravity().Length() * 3f;
ProgBlockLCD.WriteText(string.Format("Груз: {0:# ### ###} кг из {1:# ### ###} кг / {2: ##.# %}\n", curmass, maxmass, curmass / maxmass), true);
float curvol = 0;
float maxvol = 0;
IMyInventory inv;
foreach (IMyCargoContainer cargo in CargoList)
{
inv = cargo.GetInventory();
curvol += (float)inv.CurrentVolume;
maxvol += (float)inv.MaxVolume;
}
ProgBlockLCD.WriteText(string.Format("Трюм: {0:# ### ###} л из {1:# ### ###} л / {2: ##.# %}\n", curvol * 1000, maxvol * 1000, curvol / maxvol), true);
if (LCD != null)
{
string t = ProgBlockLCD.GetText();
LCD.WriteText(t);
}
}
runTime += Runtime.LastRunTimeMs;
}
void Input(string argument)
{
InputErrors.Clear();
//Legacy commands
argument = argument.ToLower().Trim();
if (argument.Length == 0)
{
//Toggle cruise control
ToggleCruiseControl();
}
else if (argument == "on")
{
StartCruiseControl(true);
}
else if (argument == "off")
{
StartCruiseControl(true);
}
else if (argument == "swap")
{
if (thrustDirection == Base6Directions.Direction.Forward)
{
thrustDirection = Base6Directions.Direction.Up;
}
else if (thrustDirection == Base6Directions.Direction.Up)
{
thrustDirection = Base6Directions.Direction.Forward;
}
if (Cruise != null && Cruise.Enabled)
{
//Stop then start to reset used thrusters
StartCruiseControl(false);
StartCruiseControl(true);
}
}
else if (argument == "reset")
{
ResetController();
return;
}
else
{
float newSpeed = 0;
if (float.TryParse(argument, out newSpeed))
{
targetSpeed = newSpeed;
StartCruiseControl(true);
}
else
{
string[] commands = argument.ToLower().Split(',');
foreach (var command in commands)
{
string[] parts = command.Trim(new char[] { ' ', '\'' }).Split(' ');
for (int i = 0; i < parts.Length; i++)
{
parts[i] = parts[i].Trim();
}
if (parts.Length > 0)
{
if (parts[0] == "?")
{
InputErrors.Add("Errors shows up here.");
}
else if (parts[0] == "cruise" || parts[0] == "cc")
{
if (parts.Length > 1)
{
float num = 0;
bool on = true;
for (int i = 1; i < parts.Length; i++)
{
if (float.TryParse(parts[i], out num))
{
if (targetSpeed == num && Cruise != null && Cruise.Enabled)
{
on = false;
}
targetSpeed = num;
}
else if (parts[i] == "on")
{
on = true;
}
else if (parts[i] == "off")
{
on = false;
}
else
{
InputErrors.Add("'" + parts[1] + "' not recognozed for 'cruise' use: on|off (or omitt for toggle).");
}
}
StartCruiseControl(on);
}
else
{
ToggleCruiseControl();
}
}
else if (parts[0] == "align" || parts[0] == "al")
{
if (parts.Length > 1)
{
bool on = true;
for (int i = 1; i < parts.Length; i++)
{
if (parts[i] == "on")
{
on = true;
}
else if (parts[i] == "off")
{
on = false;
}
else
{
InputErrors.Add("'" + parts[1] + "' not recognozed for 'align' use: on|off (or omitt for toggle).");
}
}
StartAlign(on);
}
else
{
ToggleAlign();
}
}
else if (parts[0] == "ts")
{
if (parts.Length > 1)
{
float num = 0;
if (float.TryParse(parts[1], out num))
{
targetSpeed = num;
if (Cruise != null && Cruise.Enabled)
{
StartCruiseControl(true);
}
}
else
{
InputErrors.Add("'" + parts[1] + "' not recognized as a number. 'ts' requires one (negative for descent).");
}
}
else
{
InputErrors.Add("'ts' requires a number (negative for descent).");
}
}
else if (parts[0] == "td")
{
if (parts.Length > 1)
{
switch (parts[1])
{
case "up": thrustDirection = Base6Directions.Direction.Up; break;
case "down": thrustDirection = Base6Directions.Direction.Down; break;
case "forward": thrustDirection = Base6Directions.Direction.Forward; break;
case "backward": thrustDirection = Base6Directions.Direction.Backward; break;
case "auto": thrustDirection = Base6Directions.Direction.Left; break;
default:
InputErrors.Add("'" + parts[1] + "' not recognized for 'td', use: up | down | forward | backward | auto.");
break;
}
if (Cruise != null && Cruise.Enabled)
{
StartCruiseControl(false);
StartCruiseControl(true);
}
if (Align != null && Align.Enabled)
{
StartAlign(true);
}
}
else
{
InputErrors.Add("'td' requires a direction: up | down | forward | backward | auto.");
}
}
else if (parts[0] == "mx")
{
if (parts.Length > 1)
{
double num = 0;
if (double.TryParse(parts[1], out num))
{
if (num <= 0)
{
targetAltAscending = double.MaxValue;
}
else
{
targetAltAscending = num;
}
if (Cruise != null && Cruise.Enabled)
{
StartCruiseControl(true);
}
}
else if (parts[1] == "this")
{
if (MainController != null)
{
double elev = 0;
if (MainController.TryGetPlanetElevation((useSeaLevel ? MyPlanetElevation.Sealevel : MyPlanetElevation.Surface), out elev))
{
targetAltAscending = elev;
}
}
else
{
InputErrors.Add("No controller, can set 'mx' to 'this'.");
}
}
else
{
InputErrors.Add("'mx' requires a number or 'this', couldn't parse '" + parts[1] + "'.");
}
}
else
{
InputErrors.Add("'mx' requires a number (set to -1 for unlimited) or 'this' for current altitude.");
}
}
else if (parts[0] == "mn")
{
if (parts.Length > 1)
{
double num = 0;
if (double.TryParse(parts[1], out num))
{
if (num < 0)
{
targetAltDescending = 0;
}
else
{
targetAltDescending = num;
}
if (Cruise != null && Cruise.Enabled)
{
StartCruiseControl(true);
}
}
else if (parts[1] == "this")
{
if (MainController != null)
{
double elev = 0;
if (MainController.TryGetPlanetElevation(MyPlanetElevation.Surface, out elev))
{
targetAltDescending = elev;
}
}
else
{
InputErrors.Add("No controller, can set 'mn' to 'this'.");
}
}
else
{
InputErrors.Add("'mn' requires a number or 'this', couldn't parse '" + parts[1] + "'.");
}
}
else
{
InputErrors.Add("'mn' requires a number or 'this' for current altitude.");
}
}
else if (parts[0] == "rf")
{
if (parts.Length > 1)
{
if (parts[1] == "sea" || parts[1] == "sealevel")
{
useSeaLevel = true;
}
else
{
useSeaLevel = false;
}
if (Cruise != null && Cruise.Enabled)
{
StartCruiseControl(true);
}
}
else
{
InputErrors.Add("'rf' requires a reference point: sea | ground");
}
}
else if (parts[0] == "ag")
{
if (parts.Length > 1)
{
if (parts[1] == "yes")
{
disableAlignExitingGravity = true;
}
else
{
disableAlignExitingGravity = false;
}
if (Cruise != null && Cruise.Enabled)
{
StartCruiseControl(true);
}
}
else
{
InputErrors.Add("'ag' requires a state: on | off.");
}
}
else if (parts[0] == "cg")
{
if (parts.Length > 1)
{
if (parts[1] == "yes")
{
disableCruiseExitingGravity = true;
}
else
{
disableCruiseExitingGravity = false;
}
if (Cruise != null && Cruise.Enabled)
{
StartCruiseControl(true);
}
}
else
{
InputErrors.Add("'cg' requires a state: on | off.");
}
}
else if (parts[0] == "ws")
{
if (parts.Length > 1)
{
float num = 0;
if (float.TryParse(parts[1], out num))
{
worldTopSpeed = num;
}
else
{
InputErrors.Add("'" + parts[1] + "' not recognized as a number. 'ws' requires one.");
}
}
else
{
InputErrors.Add("'ws' requires a number.");
}
}
else
{
InputErrors.Add("Command not recognized '" + parts[0] + "'");
}
}
else
{
InputErrors.Add("Input not recognized '" + command + "'");
}
}
}
}
if (InputErrors.Count > 0)
{
InputErrorTimeout = TimeSpan.FromSeconds(60) + time;
}
else
{
InputErrorTimeout = TimeSpan.MinValue;
}
SaveSettings();
}
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)");
}
}
public void Main(string arg)
{
RController = GridTerminalSystem.GetBlockWithName(RC) as IMyShipController;
if (RController == null)
{
Echo(RCFailedMSG);
RCFailed = true;
Status = "Failed";
return;
}
RControllers = GridTerminalSystem.GetBlockWithName(RC) as IMyRemoteControl;
if (RControllers == null)
{
Echo(RCFailedMSG);
RCFailed = true;
Status = "Failed";
return;
}
var CCruise = GridTerminalSystem.GetBlockWithName(CC) as IMyProgrammableBlock;
if (CCruise == null)
{
Echo(CCFailedMSG);
CCFailed = true;
Status = "Failed";
return;
}
RGyro = GridTerminalSystem.GetBlockWithName(Gyro) as IMyGyro;
if (RGyro == null)
{
Echo(GyroFailedMSG);
GyroFailed = true;
Status = "Failed";
return;
}
RGyros = GridTerminalSystem.GetBlockWithName(Gyro) as IMyFunctionalBlock;
if (RGyros == null)
{
Echo(GyroFailedMSG);
GyroFailed = true;
Status = "Failed";
return;
}
RCon = GridTerminalSystem.GetBlockWithName(Cargo) as IMyCargoContainer;
if (RCon == null)
{
Echo(RConFailedMSG);
RConFailed = true;
Status = "Failed";
return;
}
LAntenna = GridTerminalSystem.GetBlockWithName(LA) as IMyLaserAntenna;
if (LAntenna == null)
{
Echo(LAFailedMSG);
LAFailed = true;
Status = "Failed";
return;
}
LGear = GridTerminalSystem.GetBlockWithName(LG) as IMyTimerBlock;
if (LGear == null)
{
Echo(LGFailedMSG);
LGFailed = true;
Status = "Failed";
return;
}
CCUp = GridTerminalSystem.GetBlockWithName(CCU) as IMyTimerBlock;
if (CCUp == null)
{
Echo(CCTsFailedMSG);
CCTsFailed = true;
Status = "Failed";
return;
}
CCOff = GridTerminalSystem.GetBlockWithName(CCO) as IMyTimerBlock;
if (CCOff == null)
{
Echo(CCTsFailedMSG);
CCTsFailed = true;
Status = "Failed";
return;
}
CCDown = GridTerminalSystem.GetBlockWithName(CCD) as IMyTimerBlock;
if (CCDown == null)
{
Echo(CCTsFailedMSG);
CCTsFailed = true;
Status = "Failed";
return;
}
GridTerminalSystem.GetBlocksOfType(thrustersUP, x => x.WorldMatrix.Forward == RControllers.WorldMatrix.Up);
if (thrustersUP.Count == 0)
{
Echo($"Error: No lift-off thrusters were found!");
}
GridTerminalSystem.GetBlocksOfType(thrustersDOWN, x => x.WorldMatrix.Forward == RControllers.WorldMatrix.Down);
if (thrustersDOWN.Count == 0)
{
Echo($"Error: No lift-off thrusters were found!");
}
var shipMass = RController.CalculateShipMass();
TotalMass = shipMass.TotalMass;
AutoEnable = RControllers.IsAutoPilotEnabled;
RController.TryGetPlanetElevation(MyPlanetElevation.Sealevel, out Elev);
velo = RController.GetShipSpeed();
Position = RController.GetPosition();
Gravity = (RController.GetNaturalGravity().Length());
GravityG = (RController.GetNaturalGravity().Length() / 9.81);
thrustSumUP = 0;
thrustSumDOWN = 0;
foreach (var block in thrustersUP)
{
thrustSumUP += block.MaxEffectiveThrust;
}
foreach (var block in thrustersDOWN)
{
thrustSumDOWN += block.MaxEffectiveThrust;
}
if (Status == "Launched")
{
TargetGravity = (PlanetGravity * (Math.Pow((MaxR / (TargetAltitude + MinR)), 7)));
Accel = ((thrustSumUP / TotalMass) + TargetGravity); //Stopping force
AccelTime = ((0 - velo) / -Accel); //Seconds to stop
stopDistance = ((velo * AccelTime) + ((-Accel * (AccelTime * AccelTime)) / 2));
}
if (Status == "Return")
{
Accel = ((thrustSumDOWN / TotalMass) - PlanetGravity); //Stopping force
AccelTime = ((0 - velo) / -Accel); //Seconds to stop
stopDistance = ((velo * AccelTime) + ((-Accel * (AccelTime * AccelTime)) / 2));
}
Echo(Ship + " Control Pro");
Echo("Status: " + Status);
Echo("Altitude: " + Math.Round(Elev, 2) + "/" + TargetAltitude);
Echo("Speed: " + Math.Round(velo, 2));
Echo("Total Weight: " + TotalMass);
Echo("Gravity: " + Math.Round(GravityG, 2) + "G");
string msg = ("Ship" + ":" + Ship + "," + "Status" + ":" + Status + "," + "Elevation" + ":" + Elev + "," + "Position" + ":" + Position + "," + "Speed" + ":" + velo + "," + "Target" + ":" + TargetAltitude + ",");
LAntenna.TransmitMessage(msg);
var keyValuePairs = arg.Split(',').Select(x => x.Split(':')).Where(x => x.Length == 2).ToDictionary(x => x.First(), x => x.Last());
//Echo(thrustSumDOWN.ToString());
//Echo(thrustersDOWN.Count.ToString());
//Echo(thrustSumUP.ToString());
//Echo(thrustersUP.Count.ToString());
if (Status == "Failed")
{
if (RCFailed == true)
{
Echo(RCFailedMSG); return;
}
if (CCFailed == true)
{
Echo(CCFailedMSG); return;
}
if (GyroFailed == true)
{
Echo(GyroFailedMSG); return;
}
if (LAFailed == true)
{
Echo(LAFailedMSG); return;
}
if (LGFailed == true)
{
Echo(LGFailedMSG); return;
}
if (RConFailed == true)
{
Echo(RConFailedMSG); return;
}
if (CCTsFailed == true)
{
Echo(CCTsFailedMSG); return;
}
Status = "Failed";
return;
}
if (arg.Contains("Target"))
{
TargetAltitudeSetter = keyValuePairs["Target"];
TargetAltitude = int.Parse(TargetAltitudeSetter);
NotReady();
}
if (!Init)
{
Status = "Initalizing..."; NotReady();
}
if (arg == "Reset")
{
Status = "Not Ready"; NotReady();
}
if (arg == "Ready")
{
Status = "Ready"; Ready();
}
if (arg == "Launch")
{
Status = "Launching"; Launch();
}
if (arg == "Launched")
{
Status = "Launched"; Climb();
}
if (arg == "Seperate")
{
Status = "Seperation"; Seperation();
}
if (arg == "Return")
{
Status = "Return"; Return();
}
if (arg == "Approach")
{
Status = "Approaching"; Approach();
}
if (arg == "Land")
{
Status = "Landing"; Land();
}
if (arg == "Landed")
{
Status = "Landed";
}
if (Status == "Launching")
{
Launch();
}
if (Status == "Launched")
{
Climb();
}
if (Status == "Seperation")
{
Seperation();
}
if (Status == "Return")
{
Return();
}
if (Status == "Approaching")
{
Approach();
}
if (Status == "Landing")
{
Land();
}
if (Status == "Landed")
{
Status = "Landed";
}
}
public void Main(string args)
{
//Обнуление состояния по умолчанию при каждом выполнении скрипта.
Display("", nameStateLCD, true);
Display("", nameParserLCD, true);
//ПОЛУЧЕНИЕ КООРДИНАТ СЕНСОРОВ СТРУКТУРЫ ЧЕРЕЗ АНТЕННУ
//Проверяем начало аргумента, если он совпадает с нашим, то выполняем скрипт для записи координат, иначе выводим это добро на debugLCD.
if (args.StartsWith("Sensors pos"))
{
Display("", nameFloorPosStorage, true);
Display(args, nameFloorPosStorage);
}
//КНОПКА СТАРТ/СТОП/АВАРИЯ
else if (args.Equals("start"))
{
//Получение собственных координат.
GetMyPos();
gyroStabState = true;
thrustOverrideState = true;
}
else if (args.Equals("stop"))
{
gyroStabState = false;
thrustOverrideState = false;
}
else if (args.Equals("failure"))
{
gyroStabState = true;
thrustOverrideState = false;
}
//НЕПОНЯТНАЯ СИТУАЦИЯ
else
{
Display(args, nameDebugLCD);
}
//Управление гироскопами.
GyroStab(gyroStabState);
//Управление двигателями.
ThrustOverride(groupNameThrusters, thrustOverrideState);
//Выводим высоту над планетой на дисплей.
remCon = FindBlockByPartOfName(nameRemCon)[0] as IMyShipController;
double elevation;
remCon.TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation);
//Уменьшаем кол-во знаков после запятой.
elevation = Math.Round(elevation, 2);
string altitudeSurf = "AltitudeSurf: " + elevation.ToString();
Display(altitudeSurf, nameStateLCD);
remCon.TryGetPlanetElevation(MyPlanetElevation.Sealevel, out elevation);
//Уменьшаем кол-во знаков после запятой.
elevation = Math.Round(elevation, 2);
string altitudeSea = "AltitudeSea: " + elevation.ToString();
Display(altitudeSea, nameStateLCD);
}
void StabilizePod()
{
//---Get speed
currentSpeed = referenceBlock.GetShipSpeed();
//---Dir'n vectors of the reference block
var referenceMatrix = referenceBlock.WorldMatrix;
var referenceForward = referenceMatrix.Forward;
var referenceLeft = referenceMatrix.Left;
var referenceUp = referenceMatrix.Up;
var referenceOrigin = referenceMatrix.Translation;
//---Get gravity vector
gravityVec = referenceBlock.GetNaturalGravity();
gravityVecMagnitude = gravityVec.Length();
if (gravityVec.LengthSquared() == 0)
{
foreach (IMyGyro thisGyro in gyros)
{
thisGyro.GyroOverride = false;
}
shouldStabilize = false;
angleRoll = 0;
angleRoll = 0;
downSpeed = 0;
return;
}
shipVelocityVec = referenceBlock.GetShipVelocities().LinearVelocity;
if (shipVelocityVec.LengthSquared() > maxSpeed * maxSpeed)
{
maxSpeed = shipVelocityVec.Length();
}
downSpeed = VectorProjection(shipVelocityVec, gravityVec).Length() * Math.Sign(shipVelocityVec.Dot(gravityVec));
//---Determine if we should manually override brake controls
altitude = 0;
referenceBlock.TryGetPlanetElevation(MyPlanetElevation.Surface, out altitude);
altitude -= shipHeight; //adjusts for height of the ship
brakeAltitudeThreshold = GetBrakingAltitudeThreshold();
stabilizeAltitudeThreshold = brakeAltitudeThreshold + 10 * currentSpeed; //this gives us a good safety cushion for stabilization procedures
//Echo($"Braking distance: {Math.Round(brakeAltitudeThreshold).ToString()}");
if (altitude < 100 && currentSpeed < 1)
{
timeSpentStationary += timeCurrentCycle;
}
else
{
timeSpentStationary = 0;
if (altitude <= stabilizeAltitudeThreshold)
{
shouldStabilize = true;
}
else
{
shouldStabilize = false;
}
if (altitude <= brakeAltitudeThreshold)
{
shouldBrake = true;
//kills dampeners to stop their interference with landing procedures
referenceBlock.DampenersOverride = false;
}
else
{
shouldBrake = false;
}
}
if (shouldBrake)
{
if (downSpeed > descentSpeed)
{
BrakingOn();
}
else
{
if (attemptToLand)
{
BrakingThrust();
}
else
{
ShutownSystems();
}
}
}
else
{
BrakingOff();
}
Vector3D alignmentVec = new Vector3D(0, 0, 0);
//--Check if drift compensation is on
if (useDriftCompensation && downSpeed > descentSpeed)
{
alignmentVec = shipVelocityVec;
}
else
{
alignmentVec = gravityVec;
}
//---Get Roll and Pitch Angles
anglePitch = Math.Acos(MathHelper.Clamp(alignmentVec.Dot(referenceForward) / alignmentVec.Length(), -1, 1)) - Math.PI / 2;
///////////////
///
Vector3D planetRelativeLeftVec = referenceForward.Cross(alignmentVec); //w.H.i.p.L.A.s.h.1.4.1
angleRoll = Math.Acos(MathHelper.Clamp(referenceLeft.Dot(planetRelativeLeftVec) / planetRelativeLeftVec.Length(), -1, 1));
angleRoll *= Math.Sign(VectorProjection(referenceLeft, alignmentVec).Dot(alignmentVec)); //ccw is positive
anglePitch *= -1;
angleRoll *= -1;
roll_deg = Math.Round(angleRoll / Math.PI * 180);
pitch_deg = Math.Round(anglePitch / Math.PI * 180);
//---Angle controller
double rollSpeed = Math.Round(angleRoll, 2);
double pitchSpeed = Math.Round(anglePitch, 2);
//---Enforce rotation speed limit
if (Math.Abs(rollSpeed) + Math.Abs(pitchSpeed) > 2 * Math.PI)
{
double scale = 2 * Math.PI / (Math.Abs(rollSpeed) + Math.Abs(pitchSpeed));
rollSpeed *= scale;
pitchSpeed *= scale;
}
if (shouldStabilize)
{
ApplyGyroOverride(pitchSpeed, 0, -rollSpeed, gyros, referenceBlock);
}
else
{
foreach (IMyGyro thisGyro in gyros)
{
thisGyro.GyroOverride = false;
}
}
}
private void UP()
{
PX = 0; NX = 0; PY = 0; NY = 0; PZ = 0; NZ = 0; bool a = false; var b = .0; double d; if (SC.TryGetPlanetElevation(MyPlanetElevation.Sealevel, out d))
{
a = d < AA; if (a)
{
b = PA(d);
}
}
var c = SC.Orientation; foreach (var t in T)
{
var e = c.TransformDirectionInverse(Base6Directions.GetFlippedDirection(t.Orientation.Forward)); var f = TH.GetValueOrDefault(t.BlockDefinition.SubtypeId); if (f == null)
{
P.Echo("Unknown thruster type: " + t.BlockDefinition.SubtypeId); continue;
}
var g = f.GetPower(a, b); switch (e)
{
case Base6Directions.Direction.Right: PX += g; break;
case Base6Directions.Direction.Left: NX += g; break;
case Base6Directions.Direction.Up: PY += g; break;
case Base6Directions.Direction.Down: NY += g; break;
case Base6Directions.Direction.Backward: PZ += g; break;
case Base6Directions.Direction.Forward: NZ += g; break;
}
}
}