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