void AttitudeControl(FlightCtrlState s) { const float terrainOffset = 5; Vector3 yawTarget = Vector3.ProjectOnPlane(targetDirection, vesselTransform.forward); // limit "aoa" if we're moving float driftMult = 1; if (vessel.horizontalSrfSpeed * 10 > CruiseSpeed) { driftMult = Mathf.Max(Vector3.Angle(vessel.srf_velocity, yawTarget) / MaxDrift, 1); yawTarget = Vector3.RotateTowards(vessel.srf_velocity, yawTarget, MaxDrift * Mathf.Deg2Rad, 0); } float yawError = VectorUtils.SignedAngle(vesselTransform.up, yawTarget, vesselTransform.right) + (aimingMode ? 0 : weaveAdjustment); DebugLine($"yaw target: {yawTarget}, yaw error: {yawError}"); DebugLine($"drift multiplier: {driftMult}"); Vector3 baseForward = vessel.transform.up * terrainOffset; float basePitch = Mathf.Atan2( AIUtils.GetTerrainAltitude(vessel.CoM + baseForward, vessel.mainBody, false) - AIUtils.GetTerrainAltitude(vessel.CoM - baseForward, vessel.mainBody, false), terrainOffset * 2) * Mathf.Rad2Deg; float pitchAngle = basePitch + TargetPitch * Mathf.Clamp01((float)vessel.horizontalSrfSpeed / CruiseSpeed); if (aimingMode) { pitchAngle = VectorUtils.SignedAngle(vesselTransform.up, Vector3.ProjectOnPlane(targetDirection, vesselTransform.right), -vesselTransform.forward); } DebugLine($"terrain fw slope: {basePitch}, target pitch: {pitchAngle}"); float pitch = 90 - Vector3.Angle(vesselTransform.up, upDir); float pitchError = pitchAngle - pitch; Vector3 baseLateral = vessel.transform.right * terrainOffset; float baseRoll = Mathf.Atan2( AIUtils.GetTerrainAltitude(vessel.CoM + baseLateral, vessel.mainBody, false) - AIUtils.GetTerrainAltitude(vessel.CoM - baseLateral, vessel.mainBody, false), terrainOffset * 2) * Mathf.Rad2Deg; float drift = VectorUtils.SignedAngle(vesselTransform.up, Vector3.ProjectOnPlane(vessel.GetSrfVelocity(), upDir), vesselTransform.right); float bank = VectorUtils.SignedAngle(-vesselTransform.forward, upDir, -vesselTransform.right); float targetRoll = baseRoll + BankAngle * Mathf.Clamp01(drift / MaxDrift) * Mathf.Clamp01((float)vessel.srfSpeed / CruiseSpeed); float rollError = targetRoll - bank; DebugLine($"terrain sideways slope: {baseRoll}, target roll: {targetRoll}"); Vector3 localAngVel = vessel.angularVelocity; s.roll = steerMult * 0.006f * rollError - 0.4f * steerDamping * -localAngVel.y; s.pitch = ((aimingMode ? 0.02f : 0.015f) * steerMult * pitchError) - (steerDamping * -localAngVel.x); s.yaw = (((aimingMode ? 0.007f : 0.005f) * steerMult * yawError) - (steerDamping * 0.2f * -localAngVel.z)) * driftMult; s.wheelSteer = -(((aimingMode ? 0.005f : 0.003f) * steerMult * yawError) - (steerDamping * 0.1f * -localAngVel.z)); if (ManeuverRCS && (Mathf.Abs(s.roll) >= 1 || Mathf.Abs(s.pitch) >= 1 || Mathf.Abs(s.yaw) >= 1)) { vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, true); } }