public static restForPeriod ( int restArray, double vector, int resting ) : int | ||
restArray | int | |
vector | double | |
resting | int | |
return | int |
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); } } } }