public override void UpdateSolver(EngineThermodynamics ambientTherm, double altitude, Vector3d vel, double mach, bool ignited, bool oxygen, bool underwater)
{
choppercontrol = Vector3.zero;
float radar = 0;
if (HighLogic.LoadedSceneIsEditor)
{
hdg = vessel.ReferenceTransform.up;
choppercontrol.z = 0.85f;
}
else if (HighLogic.LoadedSceneIsFlight)
{
hdg = vessel.ReferenceTransform.up;
rgt = vessel.ReferenceTransform.right;
dwn = vessel.ReferenceTransform.forward;
radar = (float)vessel.radarAltitude + Vector3.Dot(vessel.upAxis, thrustTransforms[0].position - vessel.transform.position);
if (cycAuth != 0)
{
choppercontrol.x = vessel.ctrlState.roll;
choppercontrol.y = vessel.ctrlState.pitch;
choppercontrol.y -= Vector3.Dot(vel, hdg) * cycTrim * 0.01f;
choppercontrol.z = vessel.ctrlState.mainThrottle;// - * vessel.ctrlState.pitch - colDiffRollCoeff * vessel.ctrlState.roll;
//thrustTransforms[0].forward = Quaternion.AngleAxis(-choppercontrol.x * maxSwashPlateAngle, hdg) * thrustTransformVectorDefault;
//thrustTransforms[0].forward = Quaternion.AngleAxis(choppercontrol.y * maxSwashPlateAngle, rgt) * thrustTransforms[0].forward;
Vector3 upAxis = (Vector3)vessel.upAxis;
float rollangle = Vector3.SignedAngle(rgt, upAxis, hdg) - 90f;
float pitchangle = 90f - Vector3.SignedAngle(upAxis, hdg, rgt);
rollPID.Update(-rollKp, -rollKi, -rollKd, choppercontrol.x * 90, rollangle, Time.deltaTime);
pitchPID.Update(-pitchKp, -pitchKi, -pitchKd, choppercontrol.y * 90, pitchangle, Time.deltaTime);
if (colDiffRoll)//Osprey
{
choppercontrol.z += rollPID.getDrive() * cycAuth / 100 * colDiffRollCoeff;
choppercontrol.x = 0;
choppercontrol.y = pitchPID.getDrive() / 100 + pitchDiffYawCoeff * vessel.ctrlState.yaw;
choppercontrol.y *= cycAuth;
}
else if (colDiffPitch)//Chinook
{
choppercontrol.z += pitchPID.getDrive() * cycAuth / 100 * colDiffPitchCoeff;
choppercontrol.y = Vector3.Dot(vel, hdg) * cycTrim * 0.01f;
choppercontrol.x = rollPID.getDrive() / 100 + rollDiffYawCoeff * vessel.ctrlState.yaw;
choppercontrol.x *= cycAuth;
}
else
{
choppercontrol.y = pitchPID.getDrive() / 100 + pitchDiffYawCoeff * vessel.ctrlState.yaw;
choppercontrol.y *= cycAuth;
choppercontrol.x = rollPID.getDrive() / 100 + rollDiffYawCoeff * vessel.ctrlState.yaw;
choppercontrol.x *= cycAuth;
}
}
else
{
choppercontrol.x = choppercontrol.y = 0;
choppercontrol.z = vessel.ctrlState.mainThrottle;
}
}
else
{
hdg.Set(0, 1, 0);
}
Vector3 t = thrustTransforms[0].forward.normalized;
vel += Vector3.Cross(vessel.angularVelocity, thrustTransforms[0].position - vessel.transform.position);
(engineSolver as SolverRotor).UpdateFlightParams(choppercontrol, vel, hdg, t, radar, (float)ambientTherm.SpeedOfSound(1), this.thrustPercentage);
base.UpdateSolver(ambientTherm, altitude, vel, mach, ignited, oxygen, underwater);
}
