예제 #1
0
        public Vector3D GetBurnVector(Vector3D targetPosition, Vector3D targetVelocity, IMyRemoteControl core, Vector3 offset)
        {
            Vector3D targetVector = targetPosition - core.GetPosition();
            double   timeToTarget = targetVector.Length() / 100;

            return(targetVector + timeToTarget * (targetVelocity - 0.9 * core.GetShipVelocities().LinearVelocity));
        }
예제 #2
0
 Vector3D _velocity()
 {
     if (_checkOrInitRc())
     {
         return(new Vector3D());
     }
     return(rc.GetShipVelocities().LinearVelocity);
 }
예제 #3
0
        Vector3D GetLeadPosition(Vector3D targetPos, Vector3D targetVel)
        {
            Vector3D ownPos       = rc.GetPosition();
            Vector3D ownVelocity  = rc.GetShipVelocities().LinearVelocity;
            double   oldTime      = 0.01;
            Vector3D predictedPos = new Vector3D();

            for (int i = 0; i < 20; i++)
            {
                predictedPos = targetPos + targetVel * oldTime;
                Vector3D path    = predictedPos - ownPos;
                double   newTime = path.Length() / muzzleVelocity;
                if (Math.Abs(newTime - oldTime) < 0.000001)
                {
                    break;
                }
                oldTime = newTime;
            }
            return(predictedPos - ownVelocity * oldTime);
        }
예제 #4
0
 Vector3D GetBurnVector(IMyRemoteControl core, bool oldSystem)
 {
     if (oldSystem)
     {
         Vector3D targetVector = predictedHitPoint - core.GetPosition();
         targetVector.Normalize();
         targetVector = targetVector * 120;
         Vector3D velocity      = core.GetShipVelocities().LinearVelocity;
         Vector3D correctVector = targetVector - velocity;
         return(correctVector + core.GetPosition());
     }
     else
     {
         Vector3D targetVector = predictedPosition - core.GetPosition();
         return(targetVector + timeToTarget * (TargetInfo.Velocity - 0.9 * core.GetShipVelocities().LinearVelocity));
     }
 }
예제 #5
0
            public void OnFlightUpdateFrame()
            {
                if (is_enabled)
                {
                    desired   = Vector3D.Zero;
                    velocity  = control.GetShipVelocities().LinearVelocity;
                    gravity   = control.GetNaturalGravity();
                    ship_mass = control.CalculateShipMass();
                    effective_breaking_distance = Settings.ARRIVAL_DIST * (ship_mass.TotalMass / ship_mass.BaseMass);

                    tasks.OnUpdateFrame();

                    if ((flags & SpaceshipFlags.CAS) == SpaceshipFlags.CAS)
                    {
                        cas.OnUpdateFrame();
                    }
                    if ((flags & SpaceshipFlags.FD) == SpaceshipFlags.FD)
                    {
                        fd.OnUpdateFrame();
                    }

                    Vector3DLimit(ref desired, Settings.MAX_SPEED);

                    if ((flags & SpaceshipFlags.AP) == SpaceshipFlags.AP)
                    {
                        autopilot.OnUpdateFrame();
                    }

                    if ((flags & SpaceshipFlags.TM) == SpaceshipFlags.TM)
                    {
                        thrusters.OnUpdateFrame();
                    }
                    else
                    {
                        thrusters.Disable();
                    }

                    CheckConnector();
                    if (fd.prev == fd.next)
                    {
                        fd.SetFlightPlan(null);
                    }
                }
            }
예제 #6
0
            double CAF_DIST_TO_TARGET;                            //Outputs distance to target

            void CollectAndFire2(Vector3D INPUT_POINT, double INPUT_VELOCITY, double INPUT_MAX_VELOCITY, Vector3D REFPOS, IMyRemoteControl RC)
            {
                //Function Initialisation
                //--------------------------------------------------------------------
                if (C_A_F_HASRUN == false)
                {
                    //Initialise Classes And Basic System
                    CAF2_FORWARD = new Thrust_info(RC.WorldMatrix.Forward, myGridTerminal, RC.CubeGrid);
                    CAF2_UP      = new Thrust_info(RC.WorldMatrix.Up, myGridTerminal, RC.CubeGrid);
                    CAF2_RIGHT   = new Thrust_info(RC.WorldMatrix.Right, myGridTerminal, RC.CubeGrid);
                    CAFTHI       = new List <Thrust_info>()
                    {
                        CAF2_FORWARD, CAF2_UP, CAF2_RIGHT
                    };
                    myGridTerminal.GetBlocksOfType <IMyThrust>(CAF2_THRUST, block => block.CubeGrid == RC.CubeGrid);
                    C_A_F_HASRUN = true;

                    //Initialises Braking Component
                    foreach (var item in CAFTHI)
                    {
                        CAF2_BRAKING_COUNT = (item.PositiveMaxForce < CAF2_BRAKING_COUNT) ? item.PositiveMaxForce : CAF2_BRAKING_COUNT;
                        CAF2_BRAKING_COUNT = (item.NegativeMaxForce < CAF2_BRAKING_COUNT) ? item.PositiveMaxForce : CAF2_BRAKING_COUNT;
                    }
                }
                //Generating Maths To Point and decelleration information etc.
                //--------------------------------------------------------------------
                double   SHIPMASS     = Convert.ToDouble(RC.CalculateShipMass().PhysicalMass);
                Vector3D INPUT_VECTOR = Vector3D.Normalize(INPUT_POINT - REFPOS);
                double   VELOCITY     = RC.GetShipSpeed();

                CAF_DIST_TO_TARGET     = (REFPOS - INPUT_POINT).Length();
                CAF_SHIP_DECELLERATION = 0.75 * (CAF2_BRAKING_COUNT / SHIPMASS);
                CAF_STOPPING_DIST      = (((VELOCITY * VELOCITY) - (INPUT_VELOCITY * INPUT_VELOCITY))) / (2 * CAF_SHIP_DECELLERATION);

                //If Within Stopping Distance Halts Programme
                //--------------------------------------------
                if (!(CAF_DIST_TO_TARGET > (CAF_STOPPING_DIST + 0.25)) || CAF_DIST_TO_TARGET < 0.25 || VELOCITY > INPUT_MAX_VELOCITY)
                {
                    foreach (var thruster in CAF2_THRUST)
                    {
                        (thruster as IMyThrust).ThrustOverride = 0;
                    }
                    return;
                }
                //dev notes, this is the most major source of discontinuity between theorised system response

                //Reflects Vector To Cancel Orbiting
                //------------------------------------
                Vector3D DRIFT_VECTOR   = Vector3D.Normalize(RC.GetShipVelocities().LinearVelocity + RC.WorldMatrix.Forward * 0.00001);
                Vector3D R_DRIFT_VECTOR = -1 * Vector3D.Normalize(Vector3D.Reflect(DRIFT_VECTOR, INPUT_VECTOR));

                R_DRIFT_VECTOR = ((Vector3D.Dot(R_DRIFT_VECTOR, INPUT_VECTOR) < -0.3)) ? 0 * R_DRIFT_VECTOR : R_DRIFT_VECTOR;
                INPUT_VECTOR   = Vector3D.Normalize((4 * R_DRIFT_VECTOR) + INPUT_VECTOR);

                //Components Of Input Vector In FUR Axis
                //----------------------------------------
                double F_COMP_IN = Vector_Projection(INPUT_VECTOR, RC.WorldMatrix.Forward);
                double U_COMP_IN = Vector_Projection(INPUT_VECTOR, RC.WorldMatrix.Up);
                double R_COMP_IN = Vector_Projection(INPUT_VECTOR, RC.WorldMatrix.Right);

                //Calculate MAX Allowable in Each Axis & Length
                //-----------------------------------------------
                double F_COMP_MAX = (F_COMP_IN > 0) ? CAF2_FORWARD.PositiveMaxForce : -1 * CAF2_FORWARD.NegativeMaxForce;
                double U_COMP_MAX = (U_COMP_IN > 0) ? CAF2_UP.PositiveMaxForce : -1 * CAF2_UP.NegativeMaxForce;
                double R_COMP_MAX = (R_COMP_IN > 0) ? CAF2_RIGHT.PositiveMaxForce : -1 * CAF2_RIGHT.NegativeMaxForce;
                double MAX_FORCE  = Math.Sqrt(F_COMP_MAX * F_COMP_MAX + U_COMP_MAX * U_COMP_MAX + R_COMP_MAX * R_COMP_MAX);

                //Apply Length to Input Components and Calculates Smallest Multiplier
                //--------------------------------------------------------------------
                double F_COMP_PROJ = F_COMP_IN * MAX_FORCE;
                double U_COMP_PROJ = U_COMP_IN * MAX_FORCE;
                double R_COMP_PROJ = R_COMP_IN * MAX_FORCE;
                double MULTIPLIER  = 1;

                MULTIPLIER = (F_COMP_MAX / F_COMP_PROJ < MULTIPLIER) ? F_COMP_MAX / F_COMP_PROJ : MULTIPLIER;
                MULTIPLIER = (U_COMP_MAX / U_COMP_PROJ < MULTIPLIER) ? U_COMP_MAX / U_COMP_PROJ : MULTIPLIER;
                MULTIPLIER = (R_COMP_MAX / R_COMP_PROJ < MULTIPLIER) ? R_COMP_MAX / R_COMP_PROJ : MULTIPLIER;

                //Calculate Multiplied Components
                //---------------------------------
                CAF2_FORWARD.VCF = ((F_COMP_PROJ * MULTIPLIER) / F_COMP_MAX) * Math.Sign(F_COMP_MAX);
                CAF2_UP.VCF      = ((U_COMP_PROJ * MULTIPLIER) / U_COMP_MAX) * Math.Sign(U_COMP_MAX);
                CAF2_RIGHT.VCF   = ((R_COMP_PROJ * MULTIPLIER) / R_COMP_MAX) * Math.Sign(R_COMP_MAX);

                //Runs System Thrust Application
                //----------------------------------
                Dictionary <IMyThrust, float> THRUSTVALUES = new Dictionary <IMyThrust, float>();

                foreach (var thruster in CAF2_THRUST)
                {
                    THRUSTVALUES.Add((thruster as IMyThrust), 0f);
                }

                foreach (var THRUSTSYSTM in CAFTHI)
                {
                    List <IMyThrust> POSTHRUST = THRUSTSYSTM.PositiveThrusters;
                    List <IMyThrust> NEGTHRUST = THRUSTSYSTM.NegativeThrusters;
                    if (THRUSTSYSTM.VCF < 0)
                    {
                        POSTHRUST = THRUSTSYSTM.NegativeThrusters; NEGTHRUST = THRUSTSYSTM.PositiveThrusters;
                    }
                    foreach (var thruster in POSTHRUST)
                    {
                        THRUSTVALUES[thruster as IMyThrust] = (float)(Math.Abs(THRUSTSYSTM.VCF)) * (thruster as IMyThrust).MaxThrust;
                    }
                    foreach (var thruster in NEGTHRUST)
                    {
                        THRUSTVALUES[thruster as IMyThrust] = 1;
                    }                                                                               //(float)0.01001;}
                    foreach (var thruster in THRUSTVALUES)
                    {
                        thruster.Key.ThrustOverride = thruster.Value;
                    }                                                                                        //thruster.Key.ThrustOverride = thruster.Value;
                }
            }
예제 #7
0
            //Primary Generic Functions
            //==========================

            //Use For General Drone Flying:
            void RC_Manager(Vector3D TARGET, IMyRemoteControl RC, bool TURN_ONLY)
            {
                //Uses Rotation Control To Handle Max Rotational Velocity
                //---------------------------------------------------------
                if (RC.GetShipVelocities().AngularVelocity.AbsMax() > PrecisionMaxAngularVel)
                {
                    print("转动速度放缓"); RC.SetAutoPilotEnabled(false); return;
                }
                //Setup Of Common Variables
                //--------------------------------------------
                Vector3D DronePosition   = RC.GetPosition();
                Vector3D Drone_To_Target = Vector3D.Normalize(TARGET - DronePosition);
                //Override Direction Detection
                //-------------------------------
                double To_Target_Angle = Vector3D.Dot(Vector3D.Normalize(RC.GetShipVelocities().LinearVelocity), Drone_To_Target);
                double Ship_Velocity   = RC.GetShipVelocities().LinearVelocity.Length();

                //Turn Only: (Will drift ship automatically)
                //--------------------------------------------

                /*List<MyWaypointInfo> way = new List<MyWaypointInfo>();
                 *      RC.GetWaypointInfo(way);
                 *      if (way.Count>0)
                 *      {
                 *          if (way[0].Coords!= TARGET)
                 *          {
                 *              //RC.ApplyAction("AutoPilot_Off");
                 *              //RC.ClearWaypoints();
                 *          }
                 *      }*/

                if (TURN_ONLY)
                {
                    //if (way.Count <1)
                    {
                        RC.ClearWaypoints();
                        GYRO.GyroOverride = false;
                        RC.AddWaypoint(TARGET, "母船起点");
                        RC.FlightMode = FlightMode.OneWay;
                        RC.Direction  = Base6Directions.Direction.Forward;
                        RC.ApplyAction("AutoPilot_On");
                        RC.ApplyAction("CollisionAvoidance_Off");
                        RC.ControlThrusters = false;
                    }
                    return;
                }
                //Drift Cancellation Enabled:
                //-----------------------------
                if (To_Target_Angle < 0.4 && Ship_Velocity > 3)
                {
                    //Aim Gyro To Reflected Vector
                    Vector3D DRIFT_VECTOR           = Vector3D.Normalize(RC.GetShipVelocities().LinearVelocity);
                    Vector3D REFLECTED_DRIFT_VECTOR = -1 * (Vector3D.Normalize(Vector3D.Reflect(DRIFT_VECTOR, Drone_To_Target)));
                    Vector3D AIMPINGPOS             = (-1 * DRIFT_VECTOR * 500) + DronePosition;

                    //if (way.Count < 1 )
                    {
                        RC.ClearWaypoints();
                        GYRO.GyroOverride = false;
                        RC.AddWaypoint(AIMPINGPOS, "AIMPINGPOS");
                        RC.SpeedLimit = 100;
                        RC.FlightMode = FlightMode.OneWay;
                        RC.Direction  = Base6Directions.Direction.Forward;
                        RC.ApplyAction("AutoPilot_On");
                        RC.ApplyAction("CollisionAvoidance_Off");
                    }
                }

                //System Standard Operation:
                //---------------------------
                else
                {
                    //Assign To RC, Clear And Refresh Command
                    List <ITerminalAction> action = new List <ITerminalAction>();
                    RC.GetActions(action);
                    RC.ControlThrusters = true;
                    RC.ClearWaypoints();
                    GYRO.GyroOverride = false;
                    RC.AddWaypoint(TARGET, "目标");
                    RC.SpeedLimit = 100;
                    RC.FlightMode = FlightMode.OneWay;
                    RC.Direction  = Base6Directions.Direction.Forward;
                    RC.ApplyAction("AutoPilot_On");
                    RC.ApplyAction("DockingMode_Off");
                    RC.ApplyAction("CollisionAvoidance_On");
                }
            }
        public void Main(string argument, UpdateType updateSource)//main is called once per pysics update, ideally 60Hz
        {
            if (argument.ToLower() == "reset")
            {
                stage = 0;
            }
            bool valid = controller.TryGetPlanetElevation(MyPlanetElevation.Surface, out currentAltitude);

            var parts = Storage.Split(';');

            stage = int.Parse(parts[0]);

            maxTotalEffectiveThrust = 0.0f;//have to recalculate every time because it increases with alt
            foreach (var thruster in thrusters)
            {
                maxTotalEffectiveThrust += thruster.MaxEffectiveThrust;
            }
            physicalMass          = controller.CalculateShipMass().PhysicalMass;
            currentNaturalGravity = controller.GetNaturalGravity();   //
            double gravityMagnitude = currentNaturalGravity.Length(); //what a weird thing to call magnitude of a vector

            Echo($"STAGE: {stage}\nSPEED: {controller.GetShipVelocities().LinearVelocity.Length()}\nALT: {currentAltitude}");
            antenna.HudText = stage.ToString();

            if (stage == 0)
            {
                if (Freefall()) //if we are in freefall. Freefall is defined as accelerating at a rate ~= gravity, this implies thrusters are not acive. Also should check if it's heading towards or away from the gravity well. Linear alg time yay
                {
                    foreach (var thruster in thrusters)
                    {
                        thruster.ThrustOverride = 0;
                    }
                    stage++;
                    Storage = stage + ";";
                }
            }

            else if (stage == 1)
            {
                if (!Freefall())
                {
                    stage = 0;
                }
                double a = maxTotalEffectiveThrust / physicalMass - currentNaturalGravity.Length(); //acceleration with full retro burn
                double b = -controller.GetShipVelocities().LinearVelocity.Length();                 //velocity
                double c = currentAltitude;
                double thrustStopTime = -b / (2 * a);                                               //y=ax^2+bx ->  dy = 2ax+b | 0 = 2*a*x+b -> -b/2a=x

                Func <double, double> altitudeFunc = delegate(double x) {                           //x=time, y=altitude assuming full retro thrust
                    return(a * x * x + b * x + c);
                };

                double minHeight = altitudeFunc(thrustStopTime);                  //the lowest point of the trajectory IF thrust is engaged instantly
                double calculatedStoppingDistance = currentAltitude - minHeight;
                double safetyBuffer           = Math.Abs(b) * 1.25 + craftHeight; //a coefficient in front of the safety buffer feels wrong
                double safetyStoppingDistance = calculatedStoppingDistance + safetyBuffer;

                if (!controller.DampenersOverride && currentAltitude <= safetyStoppingDistance)
                {
                    stage++;
                    Storage = stage + ";";
                    controller.DampenersOverride = true;
                }
            }

            else if (stage == 2)//descent burn has been initiated, goto stg 3 when a safe speed has been reached
            {
                antenna.HudText = stage.ToString();

                if (controller.GetShipVelocities().LinearVelocity.Length() < safeVelocity)
                {
                    stage++;
                    Storage = stage + ";";
                }
            }

            else if (stage == 3)//target safe descent speed has been reached, maintain low speed until touchdown to planet
            {
                float totalThrustNeededToHover = physicalMass * (float)currentNaturalGravity.Length();
                float idealThrustPerThruster   = (float)totalThrustNeededToHover / (float)numThrusters;
                float thrustRatio = thrusters[0].MaxThrust / thrusters[0].MaxEffectiveThrust;//actual output of thrust is similar to but different from the input set by code/user.
                float adjustedThrustPerThruster = idealThrustPerThruster * thrustRatio;

                foreach (var thruster in thrusters)
                {
                    thruster.ThrustOverride = adjustedThrustPerThruster;
                }

                if (currentAltitude < touchdownHeight)
                {
                    stage++;
                    Storage = stage + ";";

                    foreach (var thruster in thrusters)
                    {
                        controller.DampenersOverride = false;
                        thruster.ThrustOverride      = 0.0f;
                    }
                }
            }

            else if (stage == 4)//touchdown, turn stuff off pls
            {
                antenna.HudText = stage.ToString();
                Echo($"TOUCHDOWN");
                stage   = 0;
                Storage = stage + ";";
                Runtime.UpdateFrequency = UpdateFrequency.None;//stop running the script
            }
            lastVelocities = controller.GetShipVelocities();
        }
예제 #9
0
            public void Update(MyDetectedEntityInfo target, Vector3D T, Vector3D PointerPos, Vector3D PointerDir)
            {
                if (!Launched)
                {
                    foreach (IMyThrust thr in thrusters)
                    {
                        thr.Enabled = true;
                        thr.ThrustOverridePercentage = 1;
                    }
                    Launched = true;
                }
                else
                {
                    counter++;
                    if (remcon.IsFunctional && (counter > VerticalDelay))
                    {
                        double   currentVelocity = remcon.GetShipVelocities().LinearVelocity.Length();
                        Vector3D targetvector    = new Vector3D();
                        if (LASER_GUIDED)
                        {
                            targetvector = ((remcon.GetPosition() - PointerPos).Dot(PointerDir) + 700) * PointerDir + PointerPos - remcon.GetPosition();
                        }
                        else
                        {
                            targetvector = FindInterceptVector(remcon.GetPosition(),
                                                               currentVelocity * INTERCEPT_COURSE,
                                                               T,
                                                               target.Velocity);
                        }

                        Vector3D trgNorm = Vector3D.Normalize(targetvector);

                        if ((target.Position - remcon.GetPosition()).Length() < WH_ARM_DIST)
                        {
                            if (currentVelocity - MyVelocity < -ACCEL_DET)
                            {
                                foreach (IMyWarhead wh in warheads)
                                {
                                    wh.Detonate();
                                }
                            }

                            MyVelocity = currentVelocity;
                        }

                        Vector3D velNorm          = Vector3D.Normalize(remcon.GetShipVelocities().LinearVelocity);
                        Vector3D CorrectionVector = Math.Max(ReflectK * trgNorm.Dot(velNorm), 1) * trgNorm - velNorm;
                        Vector3D G = remcon.GetNaturalGravity();

                        if (G.LengthSquared() == 0)
                        {
                            CorrectionVector = Math.Max(ReflectK * trgNorm.Dot(velNorm), 1) * trgNorm - velNorm;
                        }
                        else
                        {
                            if (JAVELIN)
                            {
                                //trgNorm = Vector3D.Normalize(Vector3D.Reflect(-G, trgNorm));
                                trgNorm = Vector3D.Normalize(G.Dot(trgNorm) * trgNorm * JAVELIN_CURVE - G);
                            }
                            CorrectionVector = Math.Max(ReflectK * trgNorm.Dot(velNorm), 1) * trgNorm - velNorm;
                            double A = 0;
                            foreach (IMyThrust thr in thrusters)
                            {
                                A += thr.MaxEffectiveThrust;
                            }
                            A /= remcon.CalculateShipMass().PhysicalMass;

                            Vector3D CorrectionNorm = Vector3D.Normalize(CorrectionVector);
                            //CorrectionVector = CorrectionNorm * A - G;
                            Vector3D gr = Vector3D.Reject(remcon.GetNaturalGravity(), CorrectionNorm);
                            CorrectionVector = CorrectionNorm * Math.Sqrt(A * A + gr.LengthSquared()) - gr;
                        }


                        Vector3D Axis = Vector3D.Normalize(CorrectionVector).Cross(remcon.WorldMatrix.Forward);
                        if (Axis.LengthSquared() < 0.1)
                        {
                            Axis += remcon.WorldMatrix.Backward * ROLL;
                        }
                        Axis *= GyroMult;
                        foreach (IMyGyro gyro in gyros)
                        {
                            gyro.Pitch = (float)Axis.Dot(gyro.WorldMatrix.Right);
                            gyro.Yaw   = (float)Axis.Dot(gyro.WorldMatrix.Up);
                            gyro.Roll  = (float)Axis.Dot(gyro.WorldMatrix.Backward);
                        }
                    }
                    else
                    {
                        foreach (IMyGyro gyro in gyros)
                        {
                            gyro.Pitch = 0;
                            gyro.Yaw   = 0;
                            gyro.Roll  = 0;
                        }
                    }
                }
            }
예제 #10
0
        void GuideMissile()
        {
            missilePos = shipReference.GetPosition();

            //---Get orientation vectors of our missile
            Vector3D missileFrontVec = shipReference.WorldMatrix.Forward;
            Vector3D missileLeftVec  = shipReference.WorldMatrix.Left;
            Vector3D missileUpVec    = shipReference.WorldMatrix.Up;

            //---Check if we have gravity
            double rollAngle = 0; double rollSpeed = 0;

            var remote = remotes[0] as IMyRemoteControl;

            gravVec = shipReference.GetNaturalGravity();
            double gravMagSquared = gravVec.LengthSquared();

            if (gravMagSquared != 0)
            {
                if (gravVec.Dot(missileUpVec) < 0)
                {
                    //	rollAngle = Math.PI / 2 - Math.Acos(MathHelper.Clamp(gravVec.Dot(missileLeftVec) / gravVec.Length(), -1, 1));
                }
                else
                {
                    //	rollAngle = Math.PI + Math.Acos(MathHelper.Clamp(gravVec.Dot(missileLeftVec) / gravVec.Length(), -1, 1));
                }
            }
            else
            {
                rollSpeed = 0;
            }


            //---Get travel vector
            var    missileVelocityVec = shipReference.GetShipVelocities().LinearVelocity;
            double speed = shipReference.GetShipSpeed();

            //---Find vector from missile to destinationVec
            Vector3D missileToTargetVec;

            if (speed > 10 ||)
            {
                missileToTargetVec = Vector3D.Negate(missileVelocityVec);
            }
            else
            {
                shipReference.TryGetPlanetPosition(out targetPos);
                missileToTargetVec = Vector3D.Negate(targetPos - missilePos);                // targetPos - missilePos;
            }

            //---Calc our new heading based upon our travel vector

            var headingVec = CalculateHeadingVector(missileToTargetVec, missileVelocityVec, driftCompensation);

            //---Get pitch and yaw angles
            double yawAngle; double pitchAngle;

            GetRotationAngles(headingVec, missileFrontVec, missileLeftVec, missileUpVec, out yawAngle, out pitchAngle);

            logger.Info("mttv: " + missileToTargetVec);
            logger.Info("mvv: " + missileVelocityVec);
            logger.Info("hv: " + headingVec);
            if (firstGuidance)
            {
                lastPitchAngle = pitchAngle;
                lastYawAngle   = yawAngle;
                firstGuidance  = false;
            }

            //---Angle controller
            logger.Debug(yawAngle + "|" + lastYawAngle);
            double yawSpeed   = Math.Round(proportionalConstant * yawAngle + derivativeConstant * (yawAngle - lastYawAngle) * 60, 3);
            double pitchSpeed = Math.Round(proportionalConstant * pitchAngle + derivativeConstant * (pitchAngle - lastPitchAngle) * 60, 3);

            //---Set appropriate gyro override
            if (speed <= 1E-2)
            {
                logger.Info("Fin");
                ApplyGyroOverride(0, 0, 0, gyros, shipReference);
                Runtime.UpdateFrequency = UpdateFrequency.None;
            }
            else
            {
                logger.Info("V=" + missileVelocityVec.Length());
                ApplyGyroOverride(pitchSpeed, yawSpeed, rollSpeed, gyros, shipReference);
            }

            //---Store previous values
            lastYawAngle   = yawAngle;
            lastPitchAngle = pitchAngle;
            lastRollAngle  = rollAngle;
        }
예제 #11
0
            public bool FlyTo(Vector3D position, IMyTerminalBlock reference, bool align, double speedLimit = -1)
            {
                Vector3D translationVector = position - reference.GetPosition();

                if (translationVector.Length() < 0.25)
                {
                    AllStop();
                    return(true);
                }

                double targetSpeed;

                if (speedLimit == -1)
                {
                    //TODO: modulate speed my thrust to weight ratio
                    targetSpeed = Math.Pow(translationVector.Length(), 1 / 1.6);
                }
                else
                {
                    targetSpeed = speedLimit;
                }

                Vector3D velocityDelta            = Vector3D.Normalize(translationVector) * targetSpeed - Remote.GetShipVelocities().LinearVelocity;
                Vector3D transformedVelocityDelta = Vector3D.TransformNormal(velocityDelta, MatrixD.Transpose(Remote.WorldMatrix));

                this.ManeuverService.SetThrust(new Vector3D(0, 0, transformedVelocityDelta.Z), align);
                this.ManeuverService.SetThrust(new Vector3D(transformedVelocityDelta.X, 0, 0), align);
                this.ManeuverService.SetThrust(new Vector3D(0, transformedVelocityDelta.Y, 0), align);

                return(false);
            }