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