void FlyToPosition(FlightCtrlState s, Vector3 targetPosition)
{
Vector3 localAngVel = vessel.angularVelocity * Mathf.Rad2Deg;
float terrainAltitude;
float dynPressure;
float velocity;
//Setup
terrainAltitude = (float)vessel.heightFromTerrain;
dynPressure = (float)vessel.dynamicPressurekPa;
velocity = (float)vessel.srfSpeed;
float upWeighting = pilot.UpWeighting(terrainAltitude, velocity);
//Calculating errors
ErrorData behavior = flightMode.Simulate(vesselTransform, velocityTransform, targetPosition, upDirection, upWeighting, vessel);
//Controlling
Steer steer = pilot.Simulate(behavior.pitchError, behavior.rollError, behavior.yawError, localAngVel, terrainAltitude, TimeWarp.fixedDeltaTime, dynPressure, velocity);
//Piloting
s.pitch = Mathf.Clamp(steer.pitch, -1, 1);
if (s.roll == s.rollTrim)
{
s.roll = Mathf.Clamp(steer.roll, -1, 1);
}
s.yaw = Mathf.Clamp(steer.yaw, -1, 1);
}
public Steer Simulate(float pitchError, float rollError, float yawError, UnityEngine.Vector3 angVel, float terrainAltitude, float timestep, float dynPress, float vel)
{
float speedFactor = vel / dynPress / 16; //More work needs to be done to sanitize speedFactor
if (speedFactor > 1.2f)
{
speedFactor = 1.2f;
}
float trimFactor = (float)Math.Sqrt(speedFactor);
//AdaptGains(pitchError, rollError, yawError, angVel, terrainAltitude, timestep, dynPress, vel, trimFactor);
float steerPitch = pitchPID.Simulate(pitchError, angVel.x, pIntLimit * trimFactor, timestep, speedFactor);
float steerRoll = rollPID.Simulate(rollError, angVel.y, rIntLimit, timestep, speedFactor);
if (pitchPID.IntegralZeroed) //yaw integrals should be zeroed at the same time that pitch PIDs are zeroed, because that happens in large turns
{
yawPID.ZeroIntegral();
}
float steerYaw = yawPID.Simulate(yawError, angVel.z, yIntLimit, timestep, speedFactor);
Steer steer = new Steer(steerPitch, steerRoll, steerYaw);
return(steer);
}
void FlyToPosition(FlightCtrlState s, Vector3 targetPosition)
{
Vector3d srfVel = vessel.srf_velocity;
if (srfVel != Vector3d.zero)
{
velocityTransform.rotation = Quaternion.LookRotation(srfVel, -vesselTransform.forward);
}
velocityTransform.rotation = Quaternion.AngleAxis(90, velocityTransform.right) * velocityTransform.rotation;
Vector3 localAngVel = vessel.angularVelocity * Mathf.Rad2Deg;
Vector3d targetDirection;
Vector3d targetDirectionYaw;
float yawError;
float pitchError;
float rollError;
float terrainAltitude;
float dynPressure;
float velocity;
//Setup
targetDirection = vesselTransform.InverseTransformDirection(targetPosition - velocityTransform.position).normalized;
targetDirectionYaw = targetDirection;
terrainAltitude = GetRadarAltitude();
dynPressure = (float)vessel.dynamicPressurekPa;
velocity = (float)vessel.srfSpeed;
pitchError = (float)Math.Asin(Vector3d.Dot(Vector3d.back, VectorUtils.Vector3dProjectOnPlane(targetDirection, Vector3d.right))) * Mathf.Rad2Deg;
yawError = (float)Math.Asin(Vector3d.Dot(Vector3d.right, VectorUtils.Vector3dProjectOnPlane(targetDirectionYaw, Vector3d.forward))) * Mathf.Rad2Deg;
//roll
Vector3 currentRoll = -vesselTransform.forward;
Vector3 rollTarget;
upWeighting = pilot.UpWeighting(terrainAltitude, dynPressure, velocity);
rollTarget = (targetPosition + Mathf.Clamp(upWeighting * (100f - Math.Abs(yawError * 1.6f) - (pitchError * 2.8f)), 0, float.PositiveInfinity) * upDirection) - vessel.CoM;
rollTarget = Vector3.ProjectOnPlane(rollTarget, vesselTransform.up);
rollError = VectorUtils.SignedAngle(currentRoll, rollTarget, vesselTransform.right);
Steer steer = pilot.Simulate(pitchError, rollError, yawError, localAngVel, terrainAltitude, TimeWarp.fixedDeltaTime, dynPressure, velocity);
s.pitch = Mathf.Clamp(steer.pitch, -1, 1);
if (s.roll == s.rollTrim)
{
s.roll = Mathf.Clamp(steer.roll, -1, 1);
}
s.yaw = Mathf.Clamp(steer.yaw, -1, 1);
//Debug
dynPressDebug = dynPressure;
speedFactorDebug = dynPressure * 16 / velocity;
invSpeedFactorDebug = 1 / (speedFactorDebug + Single.Epsilon);
//------------------
}
void FlyToPosition(FlightCtrlState s, Vector3 targetPosition)
{
Vector3d srfVel = vessel.srf_velocity;
if (srfVel != Vector3d.zero)
{
velocityTransform.rotation = Quaternion.LookRotation(srfVel, -vesselTransform.forward);
}
velocityTransform.rotation = Quaternion.AngleAxis(90, velocityTransform.right) * velocityTransform.rotation;
Vector3 localAngVel = vessel.angularVelocity * Mathf.Rad2Deg;
Vector3d targetDirection;
Vector3d targetDirectionYaw;
//float yawError;
//float pitchError;
//float rollError;
float terrainAltitude;
float dynPressure;
float velocity;
//Setup
targetDirection = vesselTransform.InverseTransformDirection(targetPosition - velocityTransform.position).normalized;
targetDirectionYaw = targetDirection;
terrainAltitude = GetRadarAltitude();
dynPressure = (float)vessel.dynamicPressurekPa;
velocity = (float)vessel.srfSpeed;
upWeighting = pilot.UpWeighting(terrainAltitude, dynPressure, velocity);
//Setting forced behavior for testing while there is no toggle button
flightMode.SetBehavior(1);
//Calculating errors
ErrorData behavior = flightMode.Simulate(vesselTransform, targetDirection, targetDirectionYaw, targetPosition, upDirection, upWeighting, vessel);
//Controlling
Steer steer = pilot.Simulate(behavior.pitchError, behavior.rollError, behavior.yawError, localAngVel, terrainAltitude, TimeWarp.fixedDeltaTime, dynPressure, velocity);
//Piloting
s.pitch = Mathf.Clamp(steer.pitch, -1, 1);
if (s.roll == s.rollTrim)
{
s.roll = Mathf.Clamp(steer.roll, -1, 1);
}
s.yaw = Mathf.Clamp(steer.yaw, -1, 1);
}
public Steer Simulate(float pitchError, float rollError, float yawError, UnityEngine.Vector3 angVel, float terrainAltitude, float timestep, float dynPress, float vel)
{
float speedFactor = vel / dynPress / 16; //More work needs to be done to sanitize speedFactor
if (speedFactor > 1.5f)
speedFactor = 1.5f;
float trimFactor = (float)Math.Sqrt(speedFactor);
if (!MouseAimSettings.FARLoaded) //TODO: definitely remove this in favor of adaptation that works with FAR
AdaptGains(pitchError, rollError, yawError, angVel, terrainAltitude, timestep, dynPress, vel, trimFactor);
float steerPitch = pitchPID.Simulate(pitchError, angVel.x, pIntLimit * trimFactor, timestep, speedFactor);
float steerRoll = rollPID.Simulate(rollError, angVel.y, rIntLimit, timestep, speedFactor);
if (pitchPID.IntegralZeroed) //yaw integrals should be zeroed at the same time that pitch PIDs are zeroed, because that happens in large turns
yawPID.ZeroIntegral();
float steerYaw = yawPID.Simulate(yawError, angVel.z, yIntLimit, timestep, speedFactor);
Steer steer = new Steer (steerPitch, steerRoll, steerYaw);
return steer;
}
