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