public void drive(FlightCtrlState s)
{
if (attitideActive)
{
updateAvailableTorque();
attitudeError = Math.Abs(Vector3d.Angle(attitudeGetReferenceRotation(Reference) * Target * Vector3d.forward, vessel.ReferenceTransform.up));
// Used in the drive_limit calculation
double precision = Math.Max(0.5, Math.Min(10.0, (torquePYAvailable + torqueThrustPYAvailable * s.mainThrottle) * 20.0 / MoI.magnitude));
// Direction we want to be facing
Quaternion target = attitudeGetReferenceRotation(Reference) * Target;
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.ReferenceTransform.rotation) * target);
Vector3d deltaEuler = new Vector3d(
(delta.eulerAngles.x > 180) ? (delta.eulerAngles.x - 360.0F) : delta.eulerAngles.x,
-((delta.eulerAngles.y > 180) ? (delta.eulerAngles.y - 360.0F) : delta.eulerAngles.y),
(delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z
);
Vector3d torque = new Vector3d(
torquePYAvailable + torqueThrustPYAvailable * s.mainThrottle,
torqueRAvailable,
torquePYAvailable + torqueThrustPYAvailable * s.mainThrottle
);
Vector3d inertia = Vector3d.Scale(
RTUtils.Sign(angularMomentum) * 1.8f,
Vector3d.Scale(
Vector3d.Scale(angularMomentum, angularMomentum),
RTUtils.Invert(Vector3d.Scale(torque, MoI))
)
);
// Determine the current level of error, this is then used to determine what act to apply
Vector3d err = deltaEuler * Math.PI / 180.0F;
err += RTUtils.Reorder(inertia, 132);
err.Scale(Vector3d.Scale(MoI, RTUtils.Invert(torque)));
prev_err = err;
// Make sure we are taking into account the correct timeframe we are working with
integral += err * TimeWarp.fixedDeltaTime;
// The inital value of act
// Act is ultimately what determines what the pitch, yaw and roll will be set to
// We make alterations to act between here and passing it into the pod controls
Vector3d act = Mathf.Clamp((float)attitudeError, 1.0F, 3.0F) * Kp * err;
//+ Ki * integral + Kd * deriv; //Ki and Kd are always 0 so they have been commented out
// The maximum value the controls may be
float drive_limit = Mathf.Clamp01((float)(err.magnitude * 450 / precision));
// Reduce z by reduceZfactor, z seems to act far too high in relation to x and y
act.z = act.z / 18.0F;
// Reduce all 3 axis to a maximum of drive_limit
act.x = Mathf.Clamp((float)act.x, drive_limit * -1, drive_limit);
act.y = Mathf.Clamp((float)act.y, drive_limit * -1, drive_limit);
act.z = Mathf.Clamp((float)act.z, drive_limit * -1, drive_limit);
// Met is the time in seconds from take off
double met = Planetarium.GetUniversalTime() - vessel.launchTime;
// Reduce effects of controls after launch and returns them gradually
// This helps to reduce large wobbles experienced in the first few seconds
if (met < 3.0F)
{
act = act * ((met / 3.0F) * (met / 3.0F));
}
// Averages out the act with the last few results, determined by the size of the a_avg_act array
act = RTUtils.averageVector3d(a_avg_act, act);
// Looks for rapid toggling of each axis and if found, then rest that axis for a while
// This helps prevents wobbles from getting larger
rapidRestX = RTUtils.restForPeriod(rapidToggleX, act.x, rapidRestX);
rapidRestY = RTUtils.restForPeriod(rapidToggleY, act.y, rapidRestY);
rapidRestZ = RTUtils.restForPeriod(rapidToggleZ, act.z, rapidRestZ);
// Reduce axis by rapidToggleRestFactor if rapid toggle rest has been triggered
if (rapidRestX > 0)
{
act.x = act.x / rapidToggleRestFactor;
}
if (rapidRestY > 0)
{
act.y = act.y / rapidToggleRestFactor;
}
if (rapidRestZ > 0)
{
act.z = act.z / rapidToggleRestFactor;
}
// Sets the SetFlightCtrlState for pitch, yaw and roll
if (!double.IsNaN(act.z))
{
s.roll = Mathf.Clamp((float)(act.z), -1, 1);
}
if (!double.IsNaN(act.x))
{
s.pitch = Mathf.Clamp((float)(act.x), -1, 1);
}
if (!double.IsNaN(act.y))
{
s.yaw = Mathf.Clamp((float)(act.y), -1, 1);
}
}
if (throttleActive)
{
s.mainThrottle = throttle;
if (throttle == 0)
{
throttleActive = false;
}
}
if (roverActive)
{
if (roverState.Steer)
{
if (Quaternion.Angle(roverState.roverRotation, core.vessel.ReferenceTransform.rotation) < roverState.Target)
{
s.wheelThrottle = roverState.reverse ? -throttlePID.Control(roverState.Speed - (float)vessel.horizontalSrfSpeed) : throttlePID.Control(roverState.Speed - (float)vessel.horizontalSrfSpeed);
s.wheelSteer = roverState.Steering;
}
else
{
s.wheelThrottle = 0;
s.wheelSteer = 0;
roverActive = false;
core.vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
}
}
else
{
if ((float)Vector3d.Distance(core.vessel.mainBody.position + altitude * core.vessel.mainBody.GetSurfaceNVector(roverState.latitude, roverState.longitude), core.vessel.transform.position) < roverState.Target)
{
s.wheelThrottle = roverState.reverse ? -throttlePID.Control(roverState.Speed - (float)vessel.horizontalSrfSpeed) : throttlePID.Control(roverState.Speed - (float)vessel.horizontalSrfSpeed);
s.wheelSteer = roverState.Steering;
}
else
{
s.wheelThrottle = 0;
s.wheelSteer = 0;
roverActive = false;
core.vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
}
}
}
}
