private static Tuple <double, double, double> NextVelocity(Tuple <double, double, double> position, Tuple <double, double, double> velocity) { Tuple <double, double, double> vel = velocity; double gMag = AltG(); Vector3 norm = Radar.MSLNormal(position); Tuple <double, double, double> dragDeceleration = DragDeceleration(); //dragDeceleration = VectorMath.MultiplyByNumber(dragDeceleration, 1 / 2); Tuple <double, double, double> gDirection = VectorMath.VectorToTuple(-norm); Tuple <double, double, double> g = VectorMath.MultiplyByNumber(gDirection, gMag); vel = VectorMath.Add(vel, dragDeceleration); return(VectorMath.Add(vel, g)); }
/// <summary> /// /// </summary> /// <param name="args"></param> /// static void Main(string[] args) { Connection conn = new Connection("Main"); Vessel vessel = conn.SpaceCenter().ActiveVessel; ReferenceFrame surfaceBodyHybrid = RefFrames.SurfaceBodyHybrid(conn); ReferenceFrame padRef = RefFrames.CreateLaunchPadRef(conn); ReferenceFrame hybridFrame = RefFrames.CreateHybrid(conn); ReferenceFrame bodyFrame = RefFrames.BodyFrame(conn); ReferenceFrame surfFrame = vessel.SurfaceReferenceFrame; ReferenceFrame surfVelFrame = vessel.SurfaceVelocityReferenceFrame; //padRef = vessel.Orbit.Body.ReferenceFrame; //var launchPad = RefFrames.LaunchPad; ReferenceFrame currentFrame = bodyFrame; Tuple <double, double, double> launchPadPos = vessel.Position(bodyFrame); //saves the position of the vessel on the latic aunchpad Stream <double> velVStream = conn.AddStream(() => vessel.Flight(currentFrame).VerticalSpeed); Stream <double> altStream = conn.AddStream(() => vessel.Flight(currentFrame).MeanAltitude); Stream <Tuple <double, double, double> > posStream = conn.AddStream(() => vessel.Position(currentFrame)); Stream <double> surfElevation = conn.AddStream(() => vessel.Flight(currentFrame).Elevation); Stream <Tuple <double, double, double> > velStream = conn.AddStream(() => vessel.Flight(currentFrame).Velocity); Stream <Tuple <double, double, double> > dragStream = conn.AddStream(() => vessel.Flight(surfVelFrame).Drag); Stream <float> mass = conn.AddStream(() => vessel.Mass); Stream <float> thrust = conn.AddStream(() => vessel.AvailableThrust); CelestialBody body = vessel.Orbit.Body; launchPadPos = RefFrames.LaunchPad; //PID section //altPID pID = new altPID(0.411, 0.469714286, 0.08990625,1,0); PID.KP = 0.728; PID.KI = 1.093; PID.KD = 0.0323; PID.SetPoint = -500; PID.Enabled = false; //Control section var zero = Tuple.Create(0.0, 0.0, 0.0); Vector3 up = new Vector3(1, 0, 0); var groundVelVec = new Vector3(0, 0, 0); var groundDistVec = new Vector3(0, 0, 0); var vecDifference = new Vector3(0, 0, 0); Vector3 anticipatedDist = Vector3.Zero; Vector3 sideSteering = Vector3.Zero; vessel.Control.SAS = false; vessel.Control.RCS = false; vessel.AutoPilot.Engage(); vessel.AutoPilot.ReferenceFrame = vessel.SurfaceReferenceFrame; vessel.AutoPilot.TargetDirection = VectorMath.VectorToTuple(Vector3.UnitX); Tuple <double, double, double> steeringDir = Tuple.Create(.0, .0, .0); //vessel.AutoPilot.TargetPitchAndHeading(45, 270); //vessel.Control.Throttle = 1; //System.Threading.Thread.Sleep(5000); //vessel.AutoPilot.TargetDirection = VectorMath.VectorToTuple(Vector3.UnitX); //vessel.Control.Throttle = 0; PID.RunInNewThread(thrust, mass, velVStream, vessel); Start: PID.Enabled = true; //PID.SetPoint = -3; DoLanding(conn, vessel, surfVelFrame); while (vessel.Situation.ToString() != "Landed") { var velocity = velStream.Get(); //Manage steering steeringDir = HoverslamSteering(conn, vessel, surfVelFrame); vessel.AutoPilot.TargetDirection = steeringDir; //Define position variables var position = posStream.Get(); var predictedPos = Physics.FuturePosition(position, velocity, Radar.ImpactTime()); //Slope var surfaceNormal = Radar.SurfaceNormal(predictedPos); var slope = Radar.Slope(surfaceNormal, position); //Visualize landing spot Vector3 norm = Radar.MSLNormal(position); Tuple <double, double> predictedLatLon = Tuple.Create(body.LatitudeAtPosition(predictedPos, bodyFrame), body.LongitudeAtPosition(predictedPos, bodyFrame)); Vector3 landingSpot = VectorMath.TupleToVector(body.SurfacePosition(predictedLatLon.Item1, predictedLatLon.Item2, bodyFrame)); Tuple <double, double> currentLatLon = Tuple.Create(body.LatitudeAtPosition(position, bodyFrame), body.LongitudeAtPosition(position, bodyFrame)); VectorMath.Visualize(conn, landingSpot, landingSpot + 50 * norm, bodyFrame); //Output Output.Print(slope, 1); //Save fuel and run hoverslam again if the first time was an overshoot. if (Radar.RealAltitude() > 75 && VectorMath.Length(velocity) < 6) { PID.SetPoint = -35; PID.Enabled = false; goto Start; } //Console.Clear(); System.Threading.Thread.Sleep(100); conn.Drawing().Clear(); } Console.Clear(); PID.SetPoint = -20; PID.Enabled = false; vessel.Control.Throttle = 0; System.Threading.Thread.Sleep(500); Environment.Exit(0); }