public void update() { VesselController.updateBodyPosition(); var quat = VesselController.getOrientation(); var shipup = Vector3.Transform(new Vector3(1.0f, 0.0f, 0.0f), quat); var shipleft = Vector3.Transform(new Vector3(0.0f, 0.0f, 1.0f), quat); var shipforward = Vector3.Transform(new Vector3(0.0f, 1.0f, 0.0f), quat); var fdir = shipforward + targetDirection * 1.1f; fdir.Normalize(); float xt = -Vector3.Dot(shipup, fdir); float yt = -Vector3.Dot(shipleft, fdir); float zt = -Vector3.Dot(shipup, Vector3.UnitX); //VesselController.setYaw((float)clamp(xt * 0.1, -0.1, 0.1)); //VesselController.setPitch((float)clamp(-yt * 0.1, -0.1, 0.1)); VesselController.setYaw((float)clamp(orientationYawContoller.Calculate(0.0, xt), -1.0, 1.0)); VesselController.setPitch((float)clamp(-orientationPitchContoller.Calculate(0.0, yt), -1.0, 1.0)); VesselController.setRoll((float)clamp(orientationRollContoller.Calculate(0.0, zt), -1.0, 1.0)); // Console.WriteLine(VesselController.getAngularVelocity().X); // Console.WriteLine(VesselController.getAngularVelocity().Y); // Console.WriteLine(VesselController.getAngularVelocity().Z); // Console.WriteLine(" "); }
public bool update() { var vesselVelocity = VesselController.getVelocity(); var downDirection = VesselController.getGravity(); downDirection.Normalize(); var velocity = Vector3.Dot(vesselVelocity, -downDirection); VesselDirectionController.setTargetDirection(-downDirection); VesselDirectionController.update(); var pidCalculatedThrottle = LandingSpeedPID.Calculate(TargetVelocity, velocity); Console.WriteLine("PID Result : {0}", pidCalculatedThrottle); double hoverPercentage = predictThrustPercentageForAcceleration(VesselController.getGravity().Length()) * 2.0 - 1.0; pidCalculatedThrottle -= hoverPercentage; VesselController.setThrottle(clamp(pidCalculatedThrottle, -1.0, 1.0) * 0.5 + 0.5); return(false); }
private void updateLandingBurn() { var targetRelative = Target - landingPrediction.Position; var targetRelativeNormalized = targetRelative; targetRelativeNormalized.Normalize(); var downDirection = VesselController.getGravity(); downDirection.Normalize(); var vesselVelocity = VesselController.getVelocity(); var vesselVelocityNormalized = vesselVelocity; vesselVelocityNormalized.Normalize(); var altitude = VesselController.getAltitude(); var targetAltitude = VesselController.getAltitudeAtPoint(Target); altitude -= targetAltitude; float tiltMultiplier = 0.38f; float relativeMultiplier = 0.0125f; if (altitude < 7000) { tiltMultiplier = 0.18f; relativeMultiplier = 0.0225f; } if (altitude < 5000) { tiltMultiplier = 0.08f; relativeMultiplier = 0.0825f; } if (altitude < 2500) { tiltMultiplier = 0.0f; } var desiredDirection = -vesselVelocityNormalized + targetRelativeNormalized * tiltMultiplier * Math.Min(1.0f, targetRelative.Length() * relativeMultiplier); if (vesselVelocity.Length() < 15.0) { bool upsidedown = Vector3.Dot(downDirection, vesselVelocityNormalized) < 0.0; desiredDirection = -(upsidedown ? downDirection : vesselVelocityNormalized);// + targetRelativeNormalized * 0.10f * Math.Min(1.0f, targetRelative.Length() * 0.0225f); } VesselDirectionController.setTargetDirection(desiredDirection); var velocity = Vector3.Dot(vesselVelocity, -downDirection); var pidCalculatedThrottle = landingSpeedPID.Calculate(-altitude * 0.06f - 5.0f, velocity); //Console.WriteLine("PID Result : {0}", pidCalculatedThrottle); double hoverPercentage = predictThrustPercentageForAcceleration(VesselController.getGravity().Length()) * 2.0 - 1.0; pidCalculatedThrottle -= hoverPercentage; VesselController.setThrottle(clamp(pidCalculatedThrottle, -1.0, 1.0) * 0.5 + 0.5); if (VesselController.getAltitude() < 500) { VesselController.setLandingGearState(true); } Console.WriteLine("[Landing burn] Prediction mismatch {0} Brake prediction {1}", targetRelative.Length(), brakeAltitudePrediction); }