| public static restForPeriod ( int restArray, double vector, int resting ) : int | ||
| restArray | int | |
| vector | double | |
| resting | int | |
| Résultat | int | |
public static int restForPeriod(int[] restArray, double vector, int resting)
{
int n = restArray.Length - 1; //Last elemet in the array, useful in loops and inseting element to the end
int insertPos = -1; //Position to insert a vector into
int vectorSign = Mathf.Clamp(Mathf.RoundToInt((float)vector), -1, 1); //Is our vector a negative, 0 or positive
float threshold = 0.95F; //Vector must be above this to class as hitting the upper limit
bool aboveThreshold = Mathf.Abs((float)vector) > threshold; //Determines if the input vector is above the threshold
// Decrease our resting count so we don't rest forever
if (resting > 0) resting--;
// Move all values in restArray towards 0 by 1, effectly taking 1 frame off the count
// Negative values indicate the vector was at the minimum threshold
for (int i = n; i >= 0; i--) restArray[i] = restArray[i] - Mathf.Clamp(restArray[i], -1, 1);
// See if the oldest value has reached 0, if it has move all values 1 to the left
if (restArray[0] == 0 && restArray[1] != 0)
{
for (int i = 0; i < n - 1; i++) restArray[i] = restArray[i + 1]; //shuffle everything down to the left
restArray[n] = 0; //item n has just been shifted to the left, so reset value to 0
}
// Find our position to insert the vector sign, insertPos will be -1 if no empty position is found
for (int i = n; i >= 0; i--)
{
if (restArray[i] == 0) insertPos = i;
}
// If we found a valid insert position, and the sign is different to the last sign, then insert it
if (
aboveThreshold && ( // First make sure we are above the threshold
// If in position 0, the sign is always different
(insertPos == 0) ||
// If in position 1 to n-1, then make sure the previous sign is different
// We don't want to rest if the axis is simply at the max for a while such as it may be for hard turns
(insertPos > 0 && vectorSign != Mathf.Clamp(restArray[insertPos - 1], -1, 1))
)
) // end if aboveThreshold
{
// Insert restFrameTolerance and the vectors sign
restArray[insertPos] = 90 * vectorSign;
}
// Determine if the array is full, we are above the threshold, and the sign is different to the last
if (aboveThreshold && insertPos == -1 && vectorSign != Mathf.Clamp(restArray[n], -1, 1))
{
// Array is full, remove oldest value to make room for new value
for (int i = 0; i < n - 1; i++)
{
restArray[i] = restArray[i + 1];
}
// Insert new value
restArray[n] = 90 * vectorSign;
// Sets the axis to rest for the length of time 3/4 of the frame difference between the first item and last item
resting = (int)Math.Ceiling(((Math.Abs(restArray[n]) - Math.Abs(restArray[0])) / 4.0) * 3.0);
}
// Returns number of frames to rest for, or 0 for no rest
return resting;
}
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);
}
}
}
}